<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">

<!-- 
	Copyright (C) 2007, 2008, 2009, 2010, 2011. PARP Research Group.
	<http://perception.inf.um.es>
	University of Murcia, Spain.

	This file is part of the QVision library.

	QVision is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as
	published by the Free Software Foundation, version 3 of the License.

	QVision is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with QVision. If not, see <http://www.gnu.org/licenses/>.
-->

<html><head><meta http-equiv="content-Type" content="text/html;charset=UTF-8">
<title>QVision: Qt&#39;s Image, Video and Computer Vision Library</title>
<meta name="title" content="QVision" />
<meta name="dc.title" content="QVision" />
<meta name="url" content="http://perception.inf.um.es/QVision" />
<meta name="author" content="PARP Research Group - http://perception.inf.um.es" />
<meta name="revisit-after" content="30 DAYS"/>
<meta name="robots" content="index,follow"/>
<meta name="classification" content="*">
<meta name="rating" content="Safe For Kids">
<meta name="distribution" content="GLOBAL"/>
<meta name="description" content="Qt's Image, Video and Computer Vision Library"/>
<meta name="page-topic" content="Computer Vision research and prototype programming"/>
<meta name="geo.country" content="ES" />

<!--
Keywords:
By license:		open source, gnu, lgpl, gpl, free
By theme:		computer vision, image processing, robotics, programming, source, development
By usage:		library, toolkit, framework, prototype, application
By programming specs:	object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping
Interoperability with:	Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack
Functionallity:		image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface
Main data-types:	matrix, vector, tensor, quaternion, image, polyline
Video sources:		webcam, camera, stream
Devices:		embedded, desktop computer, laptop, mini-laptop
Authors:		PARP research group. University of Murcia, Spain.
-->

<meta name="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="pragma" content="no-cache"/>
<meta http-equiv="title" content="QVision"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
<link href="tabs.css" rel="stylesheet" type="text/css" />
<link rel="shortcut icon" href="favicon.ico" />
</head><body>

<table width="100%"><tr>
	<td><a href="http://perception.inf.um.es/"><img src="parp.png" border="0" /> <big>PARP Research Group</big></a></td>
	<td align="right"><a href="http://www.um.es/"><big>Universidad de Murcia</big> <img src="um.png" border="0" /></a></td>
</tr></table>

<hr /><br />

<table width="95%" align="center"><tr><td>

<!-- Generated by Doxygen 1.6.3 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div class="navigation" id="top">
  <div class="tabs">
    <ul>
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
          <form id="FSearchBox" action="search.php" method="get">
            <img id="MSearchSelect" src="search/search.png" alt=""/>
            <input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S" 
                   onfocus="searchBox.OnSearchFieldFocus(true)" 
                   onblur="searchBox.OnSearchFieldFocus(false)"/>
          </form>
        </div>
      </li>
    </ul>
  </div>
  <div class="tabs">
    <ul>
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<h1>src/qvsfm/laSBA/sba-1.6/sba_lapack.c</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 
<a name="l00002"></a>00002 
<a name="l00003"></a>00003 
<a name="l00004"></a>00004 
<a name="l00005"></a>00005 
<a name="l00006"></a>00006 
<a name="l00007"></a>00007 
<a name="l00008"></a>00008 
<a name="l00009"></a>00009 
<a name="l00010"></a>00010 
<a name="l00011"></a>00011 
<a name="l00012"></a>00012 
<a name="l00013"></a>00013 
<a name="l00014"></a>00014 
<a name="l00015"></a>00015 
<a name="l00016"></a>00016 
<a name="l00017"></a>00017 
<a name="l00018"></a>00018 
<a name="l00019"></a>00019 
<a name="l00020"></a>00020 <span class="comment">/* A note on memory alignment:</span>
<a name="l00021"></a>00021 <span class="comment"> *</span>
<a name="l00022"></a>00022 <span class="comment"> * Several of the functions below use a piece of dynamically allocated memory</span>
<a name="l00023"></a>00023 <span class="comment"> * to store variables of different size (i.e., ints and doubles). To avoid</span>
<a name="l00024"></a>00024 <span class="comment"> * alignment problems, care must be taken so that elements that are larger</span>
<a name="l00025"></a>00025 <span class="comment"> * (doubles) are stored before smaller ones (ints). This ensures proper</span>
<a name="l00026"></a>00026 <span class="comment"> * alignment under different alignment choices made by different CPUs:</span>
<a name="l00027"></a>00027 <span class="comment"> * For instance, a double variable is aligned on x86 to 4 bytes but</span>
<a name="l00028"></a>00028 <span class="comment"> * aligned to 8 bytes on AMD64 despite having the same size of 8 bytes.</span>
<a name="l00029"></a>00029 <span class="comment"> */</span>
<a name="l00030"></a>00030 
<a name="l00031"></a>00031 <span class="preprocessor">#include &lt;stdio.h&gt;</span>
<a name="l00032"></a>00032 <span class="preprocessor">#include &lt;stdlib.h&gt;</span>
<a name="l00033"></a>00033 <span class="preprocessor">#include &lt;string.h&gt;</span>
<a name="l00034"></a>00034 <span class="preprocessor">#include &lt;math.h&gt;</span>
<a name="l00035"></a>00035 <span class="preprocessor">#include &lt;float.h&gt;</span>
<a name="l00036"></a>00036 
<a name="l00037"></a>00037 <span class="preprocessor">#include &quot;compiler.h&quot;</span>
<a name="l00038"></a>00038 <span class="preprocessor">#include &quot;sba.h&quot;</span>
<a name="l00039"></a>00039 
<a name="l00040"></a>00040 <span class="preprocessor">#ifdef SBA_APPEND_UNDERSCORE_SUFFIX</span>
<a name="l00041"></a>00041 <span class="preprocessor"></span><span class="preprocessor">#define F77_FUNC(func)    func ## _</span>
<a name="l00042"></a>00042 <span class="preprocessor"></span><span class="preprocessor">#else</span>
<a name="l00043"></a>00043 <span class="preprocessor"></span><span class="preprocessor">#define F77_FUNC(func)    func </span>
<a name="l00044"></a>00044 <span class="preprocessor"></span><span class="preprocessor">#endif </span><span class="comment">/* SBA_APPEND_UNDERSCORE_SUFFIX */</span>
<a name="l00045"></a>00045 
<a name="l00046"></a>00046 
<a name="l00047"></a>00047 <span class="comment">/* declarations of LAPACK routines employed */</span>
<a name="l00048"></a>00048 
<a name="l00049"></a>00049 <span class="comment">/* QR decomposition */</span>
<a name="l00050"></a>00050 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dgeqrf)(<span class="keywordtype">int</span> *m, <span class="keywordtype">int</span> *n, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">double</span> *tau, <span class="keywordtype">double</span> *work, <span class="keywordtype">int</span> *lwork, <span class="keywordtype">int</span> *info);
<a name="l00051"></a>00051 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dorgqr)(<span class="keywordtype">int</span> *m, <span class="keywordtype">int</span> *n, <span class="keywordtype">int</span> *k, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">double</span> *tau, <span class="keywordtype">double</span> *work, <span class="keywordtype">int</span> *lwork, <span class="keywordtype">int</span> *info);
<a name="l00052"></a>00052 
<a name="l00053"></a>00053 <span class="comment">/* solution of triangular system */</span>
<a name="l00054"></a>00054 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dtrtrs)(<span class="keywordtype">char</span> *uplo, <span class="keywordtype">char</span> *trans, <span class="keywordtype">char</span> *diag, <span class="keywordtype">int</span> *n, <span class="keywordtype">int</span> *nrhs, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">double</span> *b, <span class="keywordtype">int</span> *ldb, <span class="keywordtype">int</span> *info);
<a name="l00055"></a>00055 
<a name="l00056"></a>00056 <span class="comment">/* Cholesky decomposition, linear system solution and matrix inversion */</span>
<a name="l00057"></a>00057 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dpotf2)(<span class="keywordtype">char</span> *uplo, <span class="keywordtype">int</span> *n, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">int</span> *info); <span class="comment">/* unblocked Cholesky */</span>
<a name="l00058"></a>00058 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dpotrf)(<span class="keywordtype">char</span> *uplo, <span class="keywordtype">int</span> *n, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">int</span> *info); <span class="comment">/* block version of dpotf2 */</span>
<a name="l00059"></a>00059 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dpotrs)(<span class="keywordtype">char</span> *uplo, <span class="keywordtype">int</span> *n, <span class="keywordtype">int</span> *nrhs, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">double</span> *b, <span class="keywordtype">int</span> *ldb, <span class="keywordtype">int</span> *info);
<a name="l00060"></a>00060 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dpotri)(<span class="keywordtype">char</span> *uplo, <span class="keywordtype">int</span> *n, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">int</span> *info);
<a name="l00061"></a>00061 
<a name="l00062"></a>00062 <span class="comment">/* LU decomposition, linear system solution and matrix inversion */</span>
<a name="l00063"></a>00063 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dgetrf)(<span class="keywordtype">int</span> *m, <span class="keywordtype">int</span> *n, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">int</span> *ipiv, <span class="keywordtype">int</span> *info); <span class="comment">/* blocked LU */</span>
<a name="l00064"></a>00064 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dgetf2)(<span class="keywordtype">int</span> *m, <span class="keywordtype">int</span> *n, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">int</span> *ipiv, <span class="keywordtype">int</span> *info); <span class="comment">/* unblocked LU */</span>
<a name="l00065"></a>00065 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dgetrs)(<span class="keywordtype">char</span> *trans, <span class="keywordtype">int</span> *n, <span class="keywordtype">int</span> *nrhs, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">int</span> *ipiv, <span class="keywordtype">double</span> *b, <span class="keywordtype">int</span> *ldb, <span class="keywordtype">int</span> *info);
<a name="l00066"></a>00066 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dgetri)(<span class="keywordtype">int</span> *n, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">int</span> *ipiv, <span class="keywordtype">double</span> *work, <span class="keywordtype">int</span> *lwork, <span class="keywordtype">int</span> *info);
<a name="l00067"></a>00067 
<a name="l00068"></a>00068 <span class="comment">/* SVD */</span>
<a name="l00069"></a>00069 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dgesvd)(<span class="keywordtype">char</span> *jobu, <span class="keywordtype">char</span> *jobvt, <span class="keywordtype">int</span> *m, <span class="keywordtype">int</span> *n,
<a name="l00070"></a>00070            <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">double</span> *s, <span class="keywordtype">double</span> *u, <span class="keywordtype">int</span> *ldu,
<a name="l00071"></a>00071            <span class="keywordtype">double</span> *vt, <span class="keywordtype">int</span> *ldvt, <span class="keywordtype">double</span> *work, <span class="keywordtype">int</span> *lwork,
<a name="l00072"></a>00072            <span class="keywordtype">int</span> *info);
<a name="l00073"></a>00073 
<a name="l00074"></a>00074 <span class="comment">/* lapack 3.0 routine, faster than dgesvd() */</span>
<a name="l00075"></a>00075 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dgesdd)(<span class="keywordtype">char</span> *jobz, <span class="keywordtype">int</span> *m, <span class="keywordtype">int</span> *n, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda,
<a name="l00076"></a>00076            <span class="keywordtype">double</span> *s, <span class="keywordtype">double</span> *u, <span class="keywordtype">int</span> *ldu, <span class="keywordtype">double</span> *vt, <span class="keywordtype">int</span> *ldvt,
<a name="l00077"></a>00077            <span class="keywordtype">double</span> *work, <span class="keywordtype">int</span> *lwork, <span class="keywordtype">int</span> *iwork, <span class="keywordtype">int</span> *info);
<a name="l00078"></a>00078 
<a name="l00079"></a>00079 
<a name="l00080"></a>00080 <span class="comment">/* Bunch-Kaufman factorization of a real symmetric matrix A, solution of linear systems and matrix inverse */</span>
<a name="l00081"></a>00081 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dsytrf)(<span class="keywordtype">char</span> *uplo, <span class="keywordtype">int</span> *n, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">int</span> *ipiv, <span class="keywordtype">double</span> *work, <span class="keywordtype">int</span> *lwork, <span class="keywordtype">int</span> *info); <span class="comment">/* blocked ver. */</span>
<a name="l00082"></a>00082 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dsytrs)(<span class="keywordtype">char</span> *uplo, <span class="keywordtype">int</span> *n, <span class="keywordtype">int</span> *nrhs, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">int</span> *ipiv, <span class="keywordtype">double</span> *b, <span class="keywordtype">int</span> *ldb, <span class="keywordtype">int</span> *info);
<a name="l00083"></a>00083 <span class="keyword">extern</span> <span class="keywordtype">int</span> F77_FUNC(dsytri)(<span class="keywordtype">char</span> *uplo, <span class="keywordtype">int</span> *n, <span class="keywordtype">double</span> *a, <span class="keywordtype">int</span> *lda, <span class="keywordtype">int</span> *ipiv, <span class="keywordtype">double</span> *work, <span class="keywordtype">int</span> *info);
<a name="l00084"></a>00084 
<a name="l00085"></a>00085 
<a name="l00086"></a>00086 <span class="comment">/*</span>
<a name="l00087"></a>00087 <span class="comment"> * This function returns the solution of Ax = b</span>
<a name="l00088"></a>00088 <span class="comment"> *</span>
<a name="l00089"></a>00089 <span class="comment"> * The function is based on QR decomposition with explicit computation of Q:</span>
<a name="l00090"></a>00090 <span class="comment"> * If A=Q R with Q orthogonal and R upper triangular, the linear system becomes</span>
<a name="l00091"></a>00091 <span class="comment"> * Q R x = b or R x = Q^T b.</span>
<a name="l00092"></a>00092 <span class="comment"> *</span>
<a name="l00093"></a>00093 <span class="comment"> * A is mxm, b is mx1. Argument iscolmaj specifies whether A is</span>
<a name="l00094"></a>00094 <span class="comment"> * stored in column or row major order. Note that if iscolmaj==1</span>
<a name="l00095"></a>00095 <span class="comment"> * this function modifies A!</span>
<a name="l00096"></a>00096 <span class="comment"> *</span>
<a name="l00097"></a>00097 <span class="comment"> * The function returns 0 in case of error, 1 if successfull</span>
<a name="l00098"></a>00098 <span class="comment"> *</span>
<a name="l00099"></a>00099 <span class="comment"> * This function is often called repetitively to solve problems of identical</span>
<a name="l00100"></a>00100 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l00101"></a>00101 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l00102"></a>00102 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l00103"></a>00103 <span class="comment"> */</span>
<a name="l00104"></a>00104 <span class="keywordtype">int</span> sba_Axb_QR(<span class="keywordtype">double</span> *A, <span class="keywordtype">double</span> *B, <span class="keywordtype">double</span> *x, <span class="keywordtype">int</span> m, <span class="keywordtype">int</span> iscolmaj)
<a name="l00105"></a>00105 {
<a name="l00106"></a>00106 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l00107"></a>00107 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0, nb=0;
<a name="l00108"></a>00108 
<a name="l00109"></a>00109 <span class="keywordtype">double</span> *a, *r, *tau, *work;
<a name="l00110"></a>00110 <span class="keywordtype">int</span> a_sz, r_sz, tau_sz, tot_sz;
<a name="l00111"></a>00111 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00112"></a>00112 <span class="keywordtype">int</span> info, worksz, nrhs=1;
<a name="l00113"></a>00113 <span class="keyword">register</span> <span class="keywordtype">double</span> sum;
<a name="l00114"></a>00114    
<a name="l00115"></a>00115     <span class="keywordflow">if</span>(A==NULL){
<a name="l00116"></a>00116       <span class="keywordflow">if</span>(buf) free(buf);
<a name="l00117"></a>00117       buf=NULL;
<a name="l00118"></a>00118       buf_sz=0;
<a name="l00119"></a>00119 
<a name="l00120"></a>00120       <span class="keywordflow">return</span> 1;
<a name="l00121"></a>00121     }
<a name="l00122"></a>00122 
<a name="l00123"></a>00123     <span class="comment">/* calculate required memory size */</span>
<a name="l00124"></a>00124     a_sz=(iscolmaj)? 0 : m*m;
<a name="l00125"></a>00125     r_sz=m*m; <span class="comment">/* only the upper triangular part really needed */</span>
<a name="l00126"></a>00126     tau_sz=m;
<a name="l00127"></a>00127     <span class="keywordflow">if</span>(!nb){
<a name="l00128"></a>00128 <span class="preprocessor">#ifndef SBA_LS_SCARCE_MEMORY</span>
<a name="l00129"></a>00129 <span class="preprocessor"></span>      <span class="keywordtype">double</span> tmp;
<a name="l00130"></a>00130 
<a name="l00131"></a>00131       worksz=-1; <span class="comment">// workspace query; optimal size is returned in tmp</span>
<a name="l00132"></a>00132       F77_FUNC(dgeqrf)((<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">double</span> *)&amp;tmp, (<span class="keywordtype">int</span> *)&amp;worksz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00133"></a>00133       nb=((int)tmp)/m; <span class="comment">// optimal worksize is m*nb</span>
<a name="l00134"></a>00134 <span class="preprocessor">#else</span>
<a name="l00135"></a>00135 <span class="preprocessor"></span>      nb=1; <span class="comment">// min worksize is m</span>
<a name="l00136"></a>00136 <span class="preprocessor">#endif </span><span class="comment">/* SBA_LS_SCARCE_MEMORY */</span>
<a name="l00137"></a>00137     }
<a name="l00138"></a>00138     worksz=nb*m;
<a name="l00139"></a>00139     tot_sz=a_sz + r_sz + tau_sz + worksz;
<a name="l00140"></a>00140 
<a name="l00141"></a>00141     <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l00142"></a>00142       <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l00143"></a>00143 
<a name="l00144"></a>00144       buf_sz=tot_sz;
<a name="l00145"></a>00145       buf=(<span class="keywordtype">double</span> *)malloc(buf_sz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00146"></a>00146       <span class="keywordflow">if</span>(!buf){
<a name="l00147"></a>00147         fprintf(stderr, <span class="stringliteral">&quot;memory allocation in sba_Axb_QR() failed!\n&quot;</span>);
<a name="l00148"></a>00148         exit(1);
<a name="l00149"></a>00149       }
<a name="l00150"></a>00150     }
<a name="l00151"></a>00151 
<a name="l00152"></a>00152     <span class="keywordflow">if</span>(!iscolmaj){
<a name="l00153"></a>00153         a=buf;
<a name="l00154"></a>00154             <span class="comment">/* store A (column major!) into a */</span>
<a name="l00155"></a>00155             <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l00156"></a>00156                     <span class="keywordflow">for</span>(j=0; j&lt;m; ++j)
<a name="l00157"></a>00157                             a[i+j*m]=A[i*m+j];
<a name="l00158"></a>00158     }
<a name="l00159"></a>00159     <span class="keywordflow">else</span> a=A; <span class="comment">/* no copying required */</span>
<a name="l00160"></a>00160 
<a name="l00161"></a>00161     r=buf+a_sz;
<a name="l00162"></a>00162     tau=r+r_sz;
<a name="l00163"></a>00163     work=tau+tau_sz;
<a name="l00164"></a>00164 
<a name="l00165"></a>00165   <span class="comment">/* QR decomposition of A */</span>
<a name="l00166"></a>00166   F77_FUNC(dgeqrf)((<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, tau, work, (<span class="keywordtype">int</span> *)&amp;worksz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00167"></a>00167   <span class="comment">/* error treatment */</span>
<a name="l00168"></a>00168   <span class="keywordflow">if</span>(info!=0){
<a name="l00169"></a>00169     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00170"></a>00170       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dgeqrf in sba_Axb_QR()\n&quot;</span>, -info);
<a name="l00171"></a>00171       exit(1);
<a name="l00172"></a>00172     }
<a name="l00173"></a>00173     <span class="keywordflow">else</span>{
<a name="l00174"></a>00174       fprintf(stderr, <span class="stringliteral">&quot;Unknown LAPACK error %d for dgeqrf in sba_Axb_QR()\n&quot;</span>, info);
<a name="l00175"></a>00175       <span class="keywordflow">return</span> 0;
<a name="l00176"></a>00176     }
<a name="l00177"></a>00177   }
<a name="l00178"></a>00178 
<a name="l00179"></a>00179   <span class="comment">/* R is now stored in the upper triangular part of a; copy it in r so that dorgqr() below won&#39;t destroy it */</span>
<a name="l00180"></a>00180   <span class="keywordflow">for</span>(i=0; i&lt;r_sz; ++i)
<a name="l00181"></a>00181     r[i]=a[i];
<a name="l00182"></a>00182 
<a name="l00183"></a>00183   <span class="comment">/* compute Q using the elementary reflectors computed by the above decomposition */</span>
<a name="l00184"></a>00184   F77_FUNC(dorgqr)((<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, tau, work, (<span class="keywordtype">int</span> *)&amp;worksz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00185"></a>00185   <span class="keywordflow">if</span>(info!=0){
<a name="l00186"></a>00186     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00187"></a>00187       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dorgqr in sba_Axb_QR()\n&quot;</span>, -info);
<a name="l00188"></a>00188       exit(1);
<a name="l00189"></a>00189     }
<a name="l00190"></a>00190     <span class="keywordflow">else</span>{
<a name="l00191"></a>00191       fprintf(stderr, <span class="stringliteral">&quot;Unknown LAPACK error (%d) in sba_Axb_QR()\n&quot;</span>, info);
<a name="l00192"></a>00192       <span class="keywordflow">return</span> 0;
<a name="l00193"></a>00193     }
<a name="l00194"></a>00194   }
<a name="l00195"></a>00195 
<a name="l00196"></a>00196   <span class="comment">/* Q is now in a; compute Q^T b in x */</span>
<a name="l00197"></a>00197   <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){
<a name="l00198"></a>00198     <span class="keywordflow">for</span>(j=0, sum=0.0; j&lt;m; ++j)
<a name="l00199"></a>00199       sum+=a[i*m+j]*B[j];
<a name="l00200"></a>00200     x[i]=sum;
<a name="l00201"></a>00201   }
<a name="l00202"></a>00202 
<a name="l00203"></a>00203   <span class="comment">/* solve the linear system R x = Q^t b */</span>
<a name="l00204"></a>00204   F77_FUNC(dtrtrs)(<span class="stringliteral">&quot;U&quot;</span>, <span class="stringliteral">&quot;N&quot;</span>, <span class="stringliteral">&quot;N&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;nrhs, r, (<span class="keywordtype">int</span> *)&amp;m, x, (<span class="keywordtype">int</span> *)&amp;m, &amp;info);
<a name="l00205"></a>00205   <span class="comment">/* error treatment */</span>
<a name="l00206"></a>00206   <span class="keywordflow">if</span>(info!=0){
<a name="l00207"></a>00207     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00208"></a>00208       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_QR()\n&quot;</span>, -info);
<a name="l00209"></a>00209       exit(1);
<a name="l00210"></a>00210     }
<a name="l00211"></a>00211     <span class="keywordflow">else</span>{
<a name="l00212"></a>00212       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_QR()\n&quot;</span>, info);
<a name="l00213"></a>00213       <span class="keywordflow">return</span> 0;
<a name="l00214"></a>00214     }
<a name="l00215"></a>00215   }
<a name="l00216"></a>00216 
<a name="l00217"></a>00217         <span class="keywordflow">return</span> 1;
<a name="l00218"></a>00218 }
<a name="l00219"></a>00219 
<a name="l00220"></a>00220 <span class="comment">/*</span>
<a name="l00221"></a>00221 <span class="comment"> * This function returns the solution of Ax = b</span>
<a name="l00222"></a>00222 <span class="comment"> *</span>
<a name="l00223"></a>00223 <span class="comment"> * The function is based on QR decomposition without computation of Q:</span>
<a name="l00224"></a>00224 <span class="comment"> * If A=Q R with Q orthogonal and R upper triangular, the linear system becomes</span>
<a name="l00225"></a>00225 <span class="comment"> * (A^T A) x = A^T b or (R^T Q^T Q R) x = A^T b or (R^T R) x = A^T b.</span>
<a name="l00226"></a>00226 <span class="comment"> * This amounts to solving R^T y = A^T b for y and then R x = y for x</span>
<a name="l00227"></a>00227 <span class="comment"> * Note that Q does not need to be explicitly computed</span>
<a name="l00228"></a>00228 <span class="comment"> *</span>
<a name="l00229"></a>00229 <span class="comment"> * A is mxm, b is mx1. Argument iscolmaj specifies whether A is</span>
<a name="l00230"></a>00230 <span class="comment"> * stored in column or row major order. Note that if iscolmaj==1</span>
<a name="l00231"></a>00231 <span class="comment"> * this function modifies A!</span>
<a name="l00232"></a>00232 <span class="comment"> *</span>
<a name="l00233"></a>00233 <span class="comment"> * The function returns 0 in case of error, 1 if successfull</span>
<a name="l00234"></a>00234 <span class="comment"> *</span>
<a name="l00235"></a>00235 <span class="comment"> * This function is often called repetitively to solve problems of identical</span>
<a name="l00236"></a>00236 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l00237"></a>00237 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l00238"></a>00238 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l00239"></a>00239 <span class="comment"> */</span>
<a name="l00240"></a>00240 <span class="keywordtype">int</span> sba_Axb_QRnoQ(<span class="keywordtype">double</span> *A, <span class="keywordtype">double</span> *B, <span class="keywordtype">double</span> *x, <span class="keywordtype">int</span> m, <span class="keywordtype">int</span> iscolmaj)
<a name="l00241"></a>00241 {
<a name="l00242"></a>00242 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l00243"></a>00243 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0, nb=0;
<a name="l00244"></a>00244 
<a name="l00245"></a>00245 <span class="keywordtype">double</span> *a, *tau, *work;
<a name="l00246"></a>00246 <span class="keywordtype">int</span> a_sz, tau_sz, tot_sz;
<a name="l00247"></a>00247 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00248"></a>00248 <span class="keywordtype">int</span> info, worksz, nrhs=1;
<a name="l00249"></a>00249 <span class="keyword">register</span> <span class="keywordtype">double</span> sum;
<a name="l00250"></a>00250    
<a name="l00251"></a>00251     <span class="keywordflow">if</span>(A==NULL){
<a name="l00252"></a>00252       <span class="keywordflow">if</span>(buf) free(buf);
<a name="l00253"></a>00253       buf=NULL;
<a name="l00254"></a>00254       buf_sz=0;
<a name="l00255"></a>00255 
<a name="l00256"></a>00256       <span class="keywordflow">return</span> 1;
<a name="l00257"></a>00257     }
<a name="l00258"></a>00258 
<a name="l00259"></a>00259     <span class="comment">/* calculate required memory size */</span>
<a name="l00260"></a>00260     a_sz=(iscolmaj)? 0 : m*m;
<a name="l00261"></a>00261     tau_sz=m;
<a name="l00262"></a>00262     <span class="keywordflow">if</span>(!nb){
<a name="l00263"></a>00263 <span class="preprocessor">#ifndef SBA_LS_SCARCE_MEMORY</span>
<a name="l00264"></a>00264 <span class="preprocessor"></span>      <span class="keywordtype">double</span> tmp;
<a name="l00265"></a>00265 
<a name="l00266"></a>00266       worksz=-1; <span class="comment">// workspace query; optimal size is returned in tmp</span>
<a name="l00267"></a>00267       F77_FUNC(dgeqrf)((<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">double</span> *)&amp;tmp, (<span class="keywordtype">int</span> *)&amp;worksz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00268"></a>00268       nb=((int)tmp)/m; <span class="comment">// optimal worksize is m*nb</span>
<a name="l00269"></a>00269 <span class="preprocessor">#else</span>
<a name="l00270"></a>00270 <span class="preprocessor"></span>      nb=1; <span class="comment">// min worksize is m</span>
<a name="l00271"></a>00271 <span class="preprocessor">#endif </span><span class="comment">/* SBA_LS_SCARCE_MEMORY */</span>
<a name="l00272"></a>00272     }
<a name="l00273"></a>00273     worksz=nb*m;
<a name="l00274"></a>00274     tot_sz=a_sz + tau_sz + worksz;
<a name="l00275"></a>00275 
<a name="l00276"></a>00276     <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l00277"></a>00277       <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l00278"></a>00278 
<a name="l00279"></a>00279       buf_sz=tot_sz;
<a name="l00280"></a>00280       buf=(<span class="keywordtype">double</span> *)malloc(buf_sz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00281"></a>00281       <span class="keywordflow">if</span>(!buf){
<a name="l00282"></a>00282         fprintf(stderr, <span class="stringliteral">&quot;memory allocation in sba_Axb_QRnoQ() failed!\n&quot;</span>);
<a name="l00283"></a>00283         exit(1);
<a name="l00284"></a>00284       }
<a name="l00285"></a>00285     }
<a name="l00286"></a>00286 
<a name="l00287"></a>00287     <span class="keywordflow">if</span>(!iscolmaj){
<a name="l00288"></a>00288         a=buf;
<a name="l00289"></a>00289         <span class="comment">/* store A (column major!) into a */</span>
<a name="l00290"></a>00290         <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l00291"></a>00291                 <span class="keywordflow">for</span>(j=0; j&lt;m; ++j)
<a name="l00292"></a>00292                         a[i+j*m]=A[i*m+j];
<a name="l00293"></a>00293     }
<a name="l00294"></a>00294     <span class="keywordflow">else</span> a=A; <span class="comment">/* no copying required */</span>
<a name="l00295"></a>00295 
<a name="l00296"></a>00296     tau=buf+a_sz;
<a name="l00297"></a>00297     work=tau+tau_sz;
<a name="l00298"></a>00298 
<a name="l00299"></a>00299   <span class="comment">/* compute A^T b in x */</span>
<a name="l00300"></a>00300   <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){
<a name="l00301"></a>00301     <span class="keywordflow">for</span>(j=0, sum=0.0; j&lt;m; ++j)
<a name="l00302"></a>00302       sum+=a[i*m+j]*B[j];
<a name="l00303"></a>00303     x[i]=sum;
<a name="l00304"></a>00304   }
<a name="l00305"></a>00305 
<a name="l00306"></a>00306   <span class="comment">/* QR decomposition of A */</span>
<a name="l00307"></a>00307   F77_FUNC(dgeqrf)((<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, tau, work, (<span class="keywordtype">int</span> *)&amp;worksz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00308"></a>00308   <span class="comment">/* error treatment */</span>
<a name="l00309"></a>00309   <span class="keywordflow">if</span>(info!=0){
<a name="l00310"></a>00310     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00311"></a>00311       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dgeqrf in sba_Axb_QRnoQ()\n&quot;</span>, -info);
<a name="l00312"></a>00312       exit(1);
<a name="l00313"></a>00313     }
<a name="l00314"></a>00314     <span class="keywordflow">else</span>{
<a name="l00315"></a>00315       fprintf(stderr, <span class="stringliteral">&quot;Unknown LAPACK error %d for dgeqrf in sba_Axb_QRnoQ()\n&quot;</span>, info);
<a name="l00316"></a>00316       <span class="keywordflow">return</span> 0;
<a name="l00317"></a>00317     }
<a name="l00318"></a>00318   }
<a name="l00319"></a>00319 
<a name="l00320"></a>00320   <span class="comment">/* R is stored in the upper triangular part of a */</span>
<a name="l00321"></a>00321 
<a name="l00322"></a>00322   <span class="comment">/* solve the linear system R^T y = A^t b */</span>
<a name="l00323"></a>00323   F77_FUNC(dtrtrs)(<span class="stringliteral">&quot;U&quot;</span>, <span class="stringliteral">&quot;T&quot;</span>, <span class="stringliteral">&quot;N&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;nrhs, a, (<span class="keywordtype">int</span> *)&amp;m, x, (<span class="keywordtype">int</span> *)&amp;m, &amp;info);
<a name="l00324"></a>00324   <span class="comment">/* error treatment */</span>
<a name="l00325"></a>00325   <span class="keywordflow">if</span>(info!=0){
<a name="l00326"></a>00326     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00327"></a>00327       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_QRnoQ()\n&quot;</span>, -info);
<a name="l00328"></a>00328       exit(1);
<a name="l00329"></a>00329     }
<a name="l00330"></a>00330     <span class="keywordflow">else</span>{
<a name="l00331"></a>00331       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_QRnoQ()\n&quot;</span>, info);
<a name="l00332"></a>00332       <span class="keywordflow">return</span> 0;
<a name="l00333"></a>00333     }
<a name="l00334"></a>00334   }
<a name="l00335"></a>00335 
<a name="l00336"></a>00336   <span class="comment">/* solve the linear system R x = y */</span>
<a name="l00337"></a>00337   F77_FUNC(dtrtrs)(<span class="stringliteral">&quot;U&quot;</span>, <span class="stringliteral">&quot;N&quot;</span>, <span class="stringliteral">&quot;N&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;nrhs, a, (<span class="keywordtype">int</span> *)&amp;m, x, (<span class="keywordtype">int</span> *)&amp;m, &amp;info);
<a name="l00338"></a>00338   <span class="comment">/* error treatment */</span>
<a name="l00339"></a>00339   <span class="keywordflow">if</span>(info!=0){
<a name="l00340"></a>00340     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00341"></a>00341       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_QRnoQ()\n&quot;</span>, -info);
<a name="l00342"></a>00342       exit(1);
<a name="l00343"></a>00343     }
<a name="l00344"></a>00344     <span class="keywordflow">else</span>{
<a name="l00345"></a>00345       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_QRnoQ()\n&quot;</span>, info);
<a name="l00346"></a>00346       <span class="keywordflow">return</span> 0;
<a name="l00347"></a>00347     }
<a name="l00348"></a>00348   }
<a name="l00349"></a>00349 
<a name="l00350"></a>00350         <span class="keywordflow">return</span> 1;
<a name="l00351"></a>00351 }
<a name="l00352"></a>00352 
<a name="l00353"></a>00353 <span class="comment">/*</span>
<a name="l00354"></a>00354 <span class="comment"> * This function returns the solution of Ax=b</span>
<a name="l00355"></a>00355 <span class="comment"> *</span>
<a name="l00356"></a>00356 <span class="comment"> * The function assumes that A is symmetric &amp; positive definite and employs</span>
<a name="l00357"></a>00357 <span class="comment"> * the Cholesky decomposition:</span>
<a name="l00358"></a>00358 <span class="comment"> * If A=U^T U with U upper triangular, the system to be solved becomes</span>
<a name="l00359"></a>00359 <span class="comment"> * (U^T U) x = b</span>
<a name="l00360"></a>00360 <span class="comment"> * This amounts to solving U^T y = b for y and then U x = y for x</span>
<a name="l00361"></a>00361 <span class="comment"> *</span>
<a name="l00362"></a>00362 <span class="comment"> * A is mxm, b is mx1. Argument iscolmaj specifies whether A is</span>
<a name="l00363"></a>00363 <span class="comment"> * stored in column or row major order. Note that if iscolmaj==1</span>
<a name="l00364"></a>00364 <span class="comment"> * this function modifies A!</span>
<a name="l00365"></a>00365 <span class="comment"> *</span>
<a name="l00366"></a>00366 <span class="comment"> * The function returns 0 in case of error, 1 if successfull</span>
<a name="l00367"></a>00367 <span class="comment"> *</span>
<a name="l00368"></a>00368 <span class="comment"> * This function is often called repetitively to solve problems of identical</span>
<a name="l00369"></a>00369 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l00370"></a>00370 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l00371"></a>00371 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l00372"></a>00372 <span class="comment"> */</span>
<a name="l00373"></a>00373 <span class="keywordtype">int</span> sba_Axb_Chol(<span class="keywordtype">double</span> *A, <span class="keywordtype">double</span> *B, <span class="keywordtype">double</span> *x, <span class="keywordtype">int</span> m, <span class="keywordtype">int</span> iscolmaj)
<a name="l00374"></a>00374 {
<a name="l00375"></a>00375 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l00376"></a>00376 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0;
<a name="l00377"></a>00377 
<a name="l00378"></a>00378 <span class="keywordtype">double</span> *a;
<a name="l00379"></a>00379 <span class="keywordtype">int</span> a_sz, tot_sz;
<a name="l00380"></a>00380 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00381"></a>00381 <span class="keywordtype">int</span> info, nrhs=1;
<a name="l00382"></a>00382    
<a name="l00383"></a>00383     <span class="keywordflow">if</span>(A==NULL){
<a name="l00384"></a>00384       <span class="keywordflow">if</span>(buf) free(buf);
<a name="l00385"></a>00385       buf=NULL;
<a name="l00386"></a>00386       buf_sz=0;
<a name="l00387"></a>00387 
<a name="l00388"></a>00388       <span class="keywordflow">return</span> 1;
<a name="l00389"></a>00389     }
<a name="l00390"></a>00390 
<a name="l00391"></a>00391     <span class="comment">/* calculate required memory size */</span>
<a name="l00392"></a>00392     a_sz=(iscolmaj)? 0 : m*m;
<a name="l00393"></a>00393     tot_sz=a_sz;
<a name="l00394"></a>00394 
<a name="l00395"></a>00395     <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l00396"></a>00396       <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l00397"></a>00397 
<a name="l00398"></a>00398       buf_sz=tot_sz;
<a name="l00399"></a>00399       buf=(<span class="keywordtype">double</span> *)malloc(buf_sz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00400"></a>00400       <span class="keywordflow">if</span>(!buf){
<a name="l00401"></a>00401         fprintf(stderr, <span class="stringliteral">&quot;memory allocation in sba_Axb_Chol() failed!\n&quot;</span>);
<a name="l00402"></a>00402         exit(1);
<a name="l00403"></a>00403       }
<a name="l00404"></a>00404     }
<a name="l00405"></a>00405 
<a name="l00406"></a>00406     <span class="keywordflow">if</span>(!iscolmaj){
<a name="l00407"></a>00407         a=buf;
<a name="l00408"></a>00408 
<a name="l00409"></a>00409       <span class="comment">/* store A into a and B into x; A is assumed to be symmetric, hence</span>
<a name="l00410"></a>00410 <span class="comment">       * the column and row major order representations are the same</span>
<a name="l00411"></a>00411 <span class="comment">       */</span>
<a name="l00412"></a>00412       <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){
<a name="l00413"></a>00413         a[i]=A[i];
<a name="l00414"></a>00414         x[i]=B[i];
<a name="l00415"></a>00415       }
<a name="l00416"></a>00416       <span class="keywordflow">for</span>(j=m*m; i&lt;j; ++i) <span class="comment">// copy remaining rows; note that i is not re-initialized</span>
<a name="l00417"></a>00417         a[i]=A[i];
<a name="l00418"></a>00418     }
<a name="l00419"></a>00419     <span class="keywordflow">else</span>{ <span class="comment">/* no copying is necessary for A */</span>
<a name="l00420"></a>00420       a=A;
<a name="l00421"></a>00421       <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l00422"></a>00422         x[i]=B[i];
<a name="l00423"></a>00423     }
<a name="l00424"></a>00424 
<a name="l00425"></a>00425   <span class="comment">/* Cholesky decomposition of A */</span>
<a name="l00426"></a>00426   <span class="comment">//F77_FUNC(dpotf2)(&quot;U&quot;, (int *)&amp;m, a, (int *)&amp;m, (int *)&amp;info);</span>
<a name="l00427"></a>00427   F77_FUNC(dpotrf)(<span class="stringliteral">&quot;U&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00428"></a>00428   <span class="comment">/* error treatment */</span>
<a name="l00429"></a>00429   <span class="keywordflow">if</span>(info!=0){
<a name="l00430"></a>00430     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00431"></a>00431       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dpotf2/dpotrf in sba_Axb_Chol()\n&quot;</span>, -info);
<a name="l00432"></a>00432       exit(1);
<a name="l00433"></a>00433     }
<a name="l00434"></a>00434     <span class="keywordflow">else</span>{
<a name="l00435"></a>00435       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: the leading minor of order %d is not positive definite,\nthe factorization could not be completed for dpotf2/dpotrf in sba_Axb_Chol()\n&quot;</span>, info);
<a name="l00436"></a>00436       <span class="keywordflow">return</span> 0;
<a name="l00437"></a>00437     }
<a name="l00438"></a>00438   }
<a name="l00439"></a>00439 
<a name="l00440"></a>00440   <span class="comment">/* below are two alternative ways for solving the linear system: */</span>
<a name="l00441"></a>00441 <span class="preprocessor">#if 1</span>
<a name="l00442"></a>00442 <span class="preprocessor"></span>  <span class="comment">/* use the computed Cholesky in one lapack call */</span>
<a name="l00443"></a>00443   F77_FUNC(dpotrs)(<span class="stringliteral">&quot;U&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;nrhs, a, (<span class="keywordtype">int</span> *)&amp;m, x, (<span class="keywordtype">int</span> *)&amp;m, &amp;info);
<a name="l00444"></a>00444   <span class="keywordflow">if</span>(info&lt;0){
<a name="l00445"></a>00445     fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dpotrs in sba_Axb_Chol()\n&quot;</span>, -info);
<a name="l00446"></a>00446     exit(1);
<a name="l00447"></a>00447   }
<a name="l00448"></a>00448 <span class="preprocessor">#else</span>
<a name="l00449"></a>00449 <span class="preprocessor"></span>  <span class="comment">/* solve the linear systems U^T y = b, U x = y */</span>
<a name="l00450"></a>00450   F77_FUNC(dtrtrs)(<span class="stringliteral">&quot;U&quot;</span>, <span class="stringliteral">&quot;T&quot;</span>, <span class="stringliteral">&quot;N&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;nrhs, a, (<span class="keywordtype">int</span> *)&amp;m, x, (<span class="keywordtype">int</span> *)&amp;m, &amp;info);
<a name="l00451"></a>00451   <span class="comment">/* error treatment */</span>
<a name="l00452"></a>00452   <span class="keywordflow">if</span>(info!=0){
<a name="l00453"></a>00453     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00454"></a>00454       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_Chol()\n&quot;</span>, -info);
<a name="l00455"></a>00455       exit(1);
<a name="l00456"></a>00456     }
<a name="l00457"></a>00457     <span class="keywordflow">else</span>{
<a name="l00458"></a>00458       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_Chol()\n&quot;</span>, info);
<a name="l00459"></a>00459       <span class="keywordflow">return</span> 0;
<a name="l00460"></a>00460     }
<a name="l00461"></a>00461   }
<a name="l00462"></a>00462 
<a name="l00463"></a>00463   <span class="comment">/* solve U x = y */</span>
<a name="l00464"></a>00464   F77_FUNC(dtrtrs)(<span class="stringliteral">&quot;U&quot;</span>, <span class="stringliteral">&quot;N&quot;</span>, <span class="stringliteral">&quot;N&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;nrhs, a, (<span class="keywordtype">int</span> *)&amp;m, x, (<span class="keywordtype">int</span> *)&amp;m, &amp;info);
<a name="l00465"></a>00465   <span class="comment">/* error treatment */</span>
<a name="l00466"></a>00466   <span class="keywordflow">if</span>(info!=0){
<a name="l00467"></a>00467     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00468"></a>00468       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dtrtrs in sba_Axb_Chol()\n&quot;</span>, -info);
<a name="l00469"></a>00469       exit(1);
<a name="l00470"></a>00470     }
<a name="l00471"></a>00471     <span class="keywordflow">else</span>{
<a name="l00472"></a>00472       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in sba_Axb_Chol()\n&quot;</span>, info);
<a name="l00473"></a>00473       <span class="keywordflow">return</span> 0;
<a name="l00474"></a>00474     }
<a name="l00475"></a>00475   }
<a name="l00476"></a>00476 <span class="preprocessor">#endif </span><span class="comment">/* 1 */</span>
<a name="l00477"></a>00477 
<a name="l00478"></a>00478         <span class="keywordflow">return</span> 1;
<a name="l00479"></a>00479 }
<a name="l00480"></a>00480 
<a name="l00481"></a>00481 <span class="comment">/*</span>
<a name="l00482"></a>00482 <span class="comment"> * This function returns the solution of Ax = b</span>
<a name="l00483"></a>00483 <span class="comment"> *</span>
<a name="l00484"></a>00484 <span class="comment"> * The function employs LU decomposition:</span>
<a name="l00485"></a>00485 <span class="comment"> * If A=L U with L lower and U upper triangular, then the original system</span>
<a name="l00486"></a>00486 <span class="comment"> * amounts to solving</span>
<a name="l00487"></a>00487 <span class="comment"> * L y = b, U x = y</span>
<a name="l00488"></a>00488 <span class="comment"> *</span>
<a name="l00489"></a>00489 <span class="comment"> * A is mxm, b is mx1. Argument iscolmaj specifies whether A is</span>
<a name="l00490"></a>00490 <span class="comment"> * stored in column or row major order. Note that if iscolmaj==1</span>
<a name="l00491"></a>00491 <span class="comment"> * this function modifies A!</span>
<a name="l00492"></a>00492 <span class="comment"> *</span>
<a name="l00493"></a>00493 <span class="comment"> * The function returns 0 in case of error,</span>
<a name="l00494"></a>00494 <span class="comment"> * 1 if successfull</span>
<a name="l00495"></a>00495 <span class="comment"> *</span>
<a name="l00496"></a>00496 <span class="comment"> * This function is often called repetitively to solve problems of identical</span>
<a name="l00497"></a>00497 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l00498"></a>00498 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l00499"></a>00499 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l00500"></a>00500 <span class="comment"> */</span>
<a name="l00501"></a>00501 <span class="keywordtype">int</span> sba_Axb_LU(<span class="keywordtype">double</span> *A, <span class="keywordtype">double</span> *B, <span class="keywordtype">double</span> *x, <span class="keywordtype">int</span> m, <span class="keywordtype">int</span> iscolmaj)
<a name="l00502"></a>00502 {
<a name="l00503"></a>00503 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l00504"></a>00504 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0;
<a name="l00505"></a>00505 
<a name="l00506"></a>00506 <span class="keywordtype">int</span> a_sz, ipiv_sz, tot_sz;
<a name="l00507"></a>00507 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00508"></a>00508 <span class="keywordtype">int</span> info, *ipiv, nrhs=1;
<a name="l00509"></a>00509 <span class="keywordtype">double</span> *a;
<a name="l00510"></a>00510    
<a name="l00511"></a>00511     <span class="keywordflow">if</span>(A==NULL){
<a name="l00512"></a>00512       <span class="keywordflow">if</span>(buf) free(buf);
<a name="l00513"></a>00513       buf=NULL;
<a name="l00514"></a>00514       buf_sz=0;
<a name="l00515"></a>00515 
<a name="l00516"></a>00516       <span class="keywordflow">return</span> 1;
<a name="l00517"></a>00517     }
<a name="l00518"></a>00518 
<a name="l00519"></a>00519     <span class="comment">/* calculate required memory size */</span>
<a name="l00520"></a>00520     ipiv_sz=m;
<a name="l00521"></a>00521     a_sz=(iscolmaj)? 0 : m*m;
<a name="l00522"></a>00522     tot_sz=a_sz*<span class="keyword">sizeof</span>(double) + ipiv_sz*<span class="keyword">sizeof</span>(<span class="keywordtype">int</span>); <span class="comment">/* should be arranged in that order for proper doubles alignment */</span>
<a name="l00523"></a>00523 
<a name="l00524"></a>00524     <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l00525"></a>00525       <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l00526"></a>00526 
<a name="l00527"></a>00527       buf_sz=tot_sz;
<a name="l00528"></a>00528       buf=(<span class="keywordtype">double</span> *)malloc(buf_sz);
<a name="l00529"></a>00529       <span class="keywordflow">if</span>(!buf){
<a name="l00530"></a>00530         fprintf(stderr, <span class="stringliteral">&quot;memory allocation in sba_Axb_LU() failed!\n&quot;</span>);
<a name="l00531"></a>00531         exit(1);
<a name="l00532"></a>00532       }
<a name="l00533"></a>00533     }
<a name="l00534"></a>00534 
<a name="l00535"></a>00535     <span class="keywordflow">if</span>(!iscolmaj){
<a name="l00536"></a>00536       a=buf;
<a name="l00537"></a>00537       ipiv=(<span class="keywordtype">int</span> *)(a+a_sz);
<a name="l00538"></a>00538 
<a name="l00539"></a>00539       <span class="comment">/* store A (column major!) into a and B into x */</span>
<a name="l00540"></a>00540             <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){
<a name="l00541"></a>00541                     <span class="keywordflow">for</span>(j=0; j&lt;m; ++j)
<a name="l00542"></a>00542           a[i+j*m]=A[i*m+j];
<a name="l00543"></a>00543 
<a name="l00544"></a>00544         x[i]=B[i];
<a name="l00545"></a>00545       }
<a name="l00546"></a>00546     }
<a name="l00547"></a>00547     <span class="keywordflow">else</span>{ <span class="comment">/* no copying is necessary for A */</span>
<a name="l00548"></a>00548       a=A;
<a name="l00549"></a>00549       <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l00550"></a>00550         x[i]=B[i];
<a name="l00551"></a>00551       ipiv=(<span class="keywordtype">int</span> *)buf;
<a name="l00552"></a>00552     }
<a name="l00553"></a>00553 
<a name="l00554"></a>00554   <span class="comment">/* LU decomposition for A */</span>
<a name="l00555"></a>00555         F77_FUNC(dgetrf)((<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, ipiv, (<span class="keywordtype">int</span> *)&amp;info);  
<a name="l00556"></a>00556         <span class="keywordflow">if</span>(info!=0){
<a name="l00557"></a>00557                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l00558"></a>00558                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dgetrf illegal in sba_Axb_LU()\n&quot;</span>, -info);
<a name="l00559"></a>00559                         exit(1);
<a name="l00560"></a>00560                 }
<a name="l00561"></a>00561                 <span class="keywordflow">else</span>{
<a name="l00562"></a>00562                         fprintf(stderr, <span class="stringliteral">&quot;singular matrix A for dgetrf in sba_Axb_LU()\n&quot;</span>);
<a name="l00563"></a>00563                         <span class="keywordflow">return</span> 0;
<a name="l00564"></a>00564                 }
<a name="l00565"></a>00565         }
<a name="l00566"></a>00566 
<a name="l00567"></a>00567   <span class="comment">/* solve the system with the computed LU */</span>
<a name="l00568"></a>00568   F77_FUNC(dgetrs)(<span class="stringliteral">&quot;N&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;nrhs, a, (<span class="keywordtype">int</span> *)&amp;m, ipiv, x, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00569"></a>00569         <span class="keywordflow">if</span>(info!=0){
<a name="l00570"></a>00570                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l00571"></a>00571                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dgetrs illegal in sba_Axb_LU()\n&quot;</span>, -info);
<a name="l00572"></a>00572                         exit(1);
<a name="l00573"></a>00573                 }
<a name="l00574"></a>00574                 <span class="keywordflow">else</span>{
<a name="l00575"></a>00575                         fprintf(stderr, <span class="stringliteral">&quot;unknown error for dgetrs in sba_Axb_LU()\n&quot;</span>);
<a name="l00576"></a>00576                         <span class="keywordflow">return</span> 0;
<a name="l00577"></a>00577                 }
<a name="l00578"></a>00578         }
<a name="l00579"></a>00579 
<a name="l00580"></a>00580         <span class="keywordflow">return</span> 1;
<a name="l00581"></a>00581 }
<a name="l00582"></a>00582 
<a name="l00583"></a>00583 <span class="comment">/*</span>
<a name="l00584"></a>00584 <span class="comment"> * This function returns the solution of Ax = b</span>
<a name="l00585"></a>00585 <span class="comment"> *</span>
<a name="l00586"></a>00586 <span class="comment"> * The function is based on SVD decomposition:</span>
<a name="l00587"></a>00587 <span class="comment"> * If A=U D V^T with U, V orthogonal and D diagonal, the linear system becomes</span>
<a name="l00588"></a>00588 <span class="comment"> * (U D V^T) x = b or x=V D^{-1} U^T b</span>
<a name="l00589"></a>00589 <span class="comment"> * Note that V D^{-1} U^T is the pseudoinverse A^+</span>
<a name="l00590"></a>00590 <span class="comment"> *</span>
<a name="l00591"></a>00591 <span class="comment"> * A is mxm, b is mx1. Argument iscolmaj specifies whether A is</span>
<a name="l00592"></a>00592 <span class="comment"> * stored in column or row major order. Note that if iscolmaj==1</span>
<a name="l00593"></a>00593 <span class="comment"> * this function modifies A!</span>
<a name="l00594"></a>00594 <span class="comment"> *</span>
<a name="l00595"></a>00595 <span class="comment"> * The function returns 0 in case of error, 1 if successfull</span>
<a name="l00596"></a>00596 <span class="comment"> *</span>
<a name="l00597"></a>00597 <span class="comment"> * This function is often called repetitively to solve problems of identical</span>
<a name="l00598"></a>00598 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l00599"></a>00599 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l00600"></a>00600 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l00601"></a>00601 <span class="comment"> */</span>
<a name="l00602"></a>00602 <span class="keywordtype">int</span> sba_Axb_SVD(<span class="keywordtype">double</span> *A, <span class="keywordtype">double</span> *B, <span class="keywordtype">double</span> *x, <span class="keywordtype">int</span> m, <span class="keywordtype">int</span> iscolmaj)
<a name="l00603"></a>00603 {
<a name="l00604"></a>00604 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l00605"></a>00605 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0;
<a name="l00606"></a>00606 <span class="keyword">static</span> <span class="keywordtype">double</span> eps=-1.0;
<a name="l00607"></a>00607 
<a name="l00608"></a>00608 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00609"></a>00609 <span class="keywordtype">double</span> *a, *u, *s, *vt, *work;
<a name="l00610"></a>00610 <span class="keywordtype">int</span> a_sz, u_sz, s_sz, vt_sz, tot_sz;
<a name="l00611"></a>00611 <span class="keywordtype">double</span> thresh, one_over_denom;
<a name="l00612"></a>00612 <span class="keyword">register</span> <span class="keywordtype">double</span> sum;
<a name="l00613"></a>00613 <span class="keywordtype">int</span> info, rank, worksz, *iwork, iworksz;
<a name="l00614"></a>00614    
<a name="l00615"></a>00615   <span class="keywordflow">if</span>(A==NULL){
<a name="l00616"></a>00616     <span class="keywordflow">if</span>(buf) free(buf);
<a name="l00617"></a>00617     buf=NULL;
<a name="l00618"></a>00618     buf_sz=0;
<a name="l00619"></a>00619 
<a name="l00620"></a>00620     <span class="keywordflow">return</span> 1;
<a name="l00621"></a>00621   }
<a name="l00622"></a>00622 
<a name="l00623"></a>00623   <span class="comment">/* calculate required memory size */</span>
<a name="l00624"></a>00624 <span class="preprocessor">#ifndef SBA_LS_SCARCE_MEMORY</span>
<a name="l00625"></a>00625 <span class="preprocessor"></span>  worksz=-1; <span class="comment">// workspace query. Keep in mind that dgesdd requires more memory than dgesvd</span>
<a name="l00626"></a>00626   <span class="comment">/* note that optimal work size is returned in thresh */</span>
<a name="l00627"></a>00627   F77_FUNC(dgesdd)(<span class="stringliteral">&quot;A&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">int</span> *)&amp;m, NULL, NULL, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">int</span> *)&amp;m,
<a name="l00628"></a>00628           (<span class="keywordtype">double</span> *)&amp;thresh, (<span class="keywordtype">int</span> *)&amp;worksz, NULL, &amp;info);
<a name="l00629"></a>00629   <span class="comment">/* F77_FUNC(dgesvd)(&quot;A&quot;, &quot;A&quot;, (int *)&amp;m, (int *)&amp;m, NULL, (int *)&amp;m, NULL, NULL, (int *)&amp;m, NULL, (int *)&amp;m,</span>
<a name="l00630"></a>00630 <span class="comment">          (double *)&amp;thresh, (int *)&amp;worksz, &amp;info); */</span>
<a name="l00631"></a>00631   worksz=(int)thresh;
<a name="l00632"></a>00632 <span class="preprocessor">#else</span>
<a name="l00633"></a>00633 <span class="preprocessor"></span>  worksz=m*(7*m+4); <span class="comment">// min worksize for dgesdd</span>
<a name="l00634"></a>00634   <span class="comment">//worksz=5*m; // min worksize for dgesvd</span>
<a name="l00635"></a>00635 <span class="preprocessor">#endif </span><span class="comment">/* SBA_LS_SCARCE_MEMORY */</span>
<a name="l00636"></a>00636   iworksz=8*m;
<a name="l00637"></a>00637   a_sz=(!iscolmaj)? m*m : 0;
<a name="l00638"></a>00638   u_sz=m*m; s_sz=m; vt_sz=m*m;
<a name="l00639"></a>00639 
<a name="l00640"></a>00640   tot_sz=(a_sz + u_sz + s_sz + vt_sz + worksz)*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>) + iworksz*<span class="keyword">sizeof</span>(int); <span class="comment">/* should be arranged in that order for proper doubles alignment */</span>
<a name="l00641"></a>00641 
<a name="l00642"></a>00642   <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l00643"></a>00643     <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l00644"></a>00644 
<a name="l00645"></a>00645     buf_sz=tot_sz;
<a name="l00646"></a>00646     buf=(<span class="keywordtype">double</span> *)malloc(buf_sz);
<a name="l00647"></a>00647     <span class="keywordflow">if</span>(!buf){
<a name="l00648"></a>00648       fprintf(stderr, <span class="stringliteral">&quot;memory allocation in sba_Axb_SVD() failed!\n&quot;</span>);
<a name="l00649"></a>00649       exit(1);
<a name="l00650"></a>00650     }
<a name="l00651"></a>00651   }
<a name="l00652"></a>00652 
<a name="l00653"></a>00653   <span class="keywordflow">if</span>(!iscolmaj){
<a name="l00654"></a>00654     a=buf;
<a name="l00655"></a>00655     u=a+a_sz;
<a name="l00656"></a>00656     <span class="comment">/* store A (column major!) into a */</span>
<a name="l00657"></a>00657     <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l00658"></a>00658       <span class="keywordflow">for</span>(j=0; j&lt;m; ++j)
<a name="l00659"></a>00659         a[i+j*m]=A[i*m+j];
<a name="l00660"></a>00660   }
<a name="l00661"></a>00661   <span class="keywordflow">else</span>{
<a name="l00662"></a>00662     a=A; <span class="comment">/* no copying required */</span>
<a name="l00663"></a>00663     u=buf;
<a name="l00664"></a>00664   }
<a name="l00665"></a>00665 
<a name="l00666"></a>00666   s=u+u_sz;
<a name="l00667"></a>00667   vt=s+s_sz;
<a name="l00668"></a>00668   work=vt+vt_sz;
<a name="l00669"></a>00669   iwork=(<span class="keywordtype">int</span> *)(work+worksz);
<a name="l00670"></a>00670 
<a name="l00671"></a>00671   <span class="comment">/* SVD decomposition of A */</span>
<a name="l00672"></a>00672   F77_FUNC(dgesdd)(<span class="stringliteral">&quot;A&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, s, u, (<span class="keywordtype">int</span> *)&amp;m, vt, (<span class="keywordtype">int</span> *)&amp;m, work, (<span class="keywordtype">int</span> *)&amp;worksz, iwork, &amp;info);
<a name="l00673"></a>00673   <span class="comment">//F77_FUNC(dgesvd)(&quot;A&quot;, &quot;A&quot;, (int *)&amp;m, (int *)&amp;m, a, (int *)&amp;m, s, u, (int *)&amp;m, vt, (int *)&amp;m, work, (int *)&amp;worksz, &amp;info);</span>
<a name="l00674"></a>00674 
<a name="l00675"></a>00675   <span class="comment">/* error treatment */</span>
<a name="l00676"></a>00676   <span class="keywordflow">if</span>(info!=0){
<a name="l00677"></a>00677     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00678"></a>00678       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dgesdd/dgesvd in sba_Axb_SVD()\n&quot;</span>, -info);
<a name="l00679"></a>00679       exit(1);
<a name="l00680"></a>00680     }
<a name="l00681"></a>00681     <span class="keywordflow">else</span>{
<a name="l00682"></a>00682       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in sba_Axb_SVD() [info=%d]\n&quot;</span>, info);
<a name="l00683"></a>00683 
<a name="l00684"></a>00684       <span class="keywordflow">return</span> 0;
<a name="l00685"></a>00685     }
<a name="l00686"></a>00686   }
<a name="l00687"></a>00687 
<a name="l00688"></a>00688   <span class="keywordflow">if</span>(eps&lt;0.0){
<a name="l00689"></a>00689     <span class="keywordtype">double</span> aux;
<a name="l00690"></a>00690 
<a name="l00691"></a>00691     <span class="comment">/* compute machine epsilon. DBL_EPSILON should do also */</span>
<a name="l00692"></a>00692     <span class="keywordflow">for</span>(eps=1.0; aux=eps+1.0, aux-1.0&gt;0.0; eps*=0.5)
<a name="l00693"></a>00693                               ;
<a name="l00694"></a>00694     eps*=2.0;
<a name="l00695"></a>00695   }
<a name="l00696"></a>00696 
<a name="l00697"></a>00697   <span class="comment">/* compute the pseudoinverse in a */</span>
<a name="l00698"></a>00698   memset(a, 0, m*m*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>)); <span class="comment">/* initialize to zero */</span>
<a name="l00699"></a>00699   <span class="keywordflow">for</span>(rank=0, thresh=eps*s[0]; rank&lt;m &amp;&amp; s[rank]&gt;thresh; ++rank){
<a name="l00700"></a>00700     one_over_denom=1.0/s[rank];
<a name="l00701"></a>00701 
<a name="l00702"></a>00702     <span class="keywordflow">for</span>(j=0; j&lt;m; ++j)
<a name="l00703"></a>00703       <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l00704"></a>00704         a[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;
<a name="l00705"></a>00705   }
<a name="l00706"></a>00706 
<a name="l00707"></a>00707         <span class="comment">/* compute A^+ b in x */</span>
<a name="l00708"></a>00708         <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){
<a name="l00709"></a>00709           <span class="keywordflow">for</span>(j=0, sum=0.0; j&lt;m; ++j)
<a name="l00710"></a>00710       sum+=a[i*m+j]*B[j];
<a name="l00711"></a>00711     x[i]=sum;
<a name="l00712"></a>00712   }
<a name="l00713"></a>00713 
<a name="l00714"></a>00714         <span class="keywordflow">return</span> 1;
<a name="l00715"></a>00715 }
<a name="l00716"></a>00716 
<a name="l00717"></a>00717 <span class="comment">/*</span>
<a name="l00718"></a>00718 <span class="comment"> * This function returns the solution of Ax = b for a real symmetric matrix A</span>
<a name="l00719"></a>00719 <span class="comment"> *</span>
<a name="l00720"></a>00720 <span class="comment"> * The function is based on UDUT factorization with the pivoting</span>
<a name="l00721"></a>00721 <span class="comment"> * strategy of Bunch and Kaufman:</span>
<a name="l00722"></a>00722 <span class="comment"> * A is factored as U*D*U^T where U is upper triangular and</span>
<a name="l00723"></a>00723 <span class="comment"> * D symmetric and block diagonal (aka spectral decomposition,</span>
<a name="l00724"></a>00724 <span class="comment"> * Banachiewicz factorization, modified Cholesky factorization)</span>
<a name="l00725"></a>00725 <span class="comment"> *</span>
<a name="l00726"></a>00726 <span class="comment"> * A is mxm, b is mx1. Argument iscolmaj specifies whether A is</span>
<a name="l00727"></a>00727 <span class="comment"> * stored in column or row major order. Note that if iscolmaj==1</span>
<a name="l00728"></a>00728 <span class="comment"> * this function modifies A!</span>
<a name="l00729"></a>00729 <span class="comment"> *</span>
<a name="l00730"></a>00730 <span class="comment"> * The function returns 0 in case of error,</span>
<a name="l00731"></a>00731 <span class="comment"> * 1 if successfull</span>
<a name="l00732"></a>00732 <span class="comment"> *</span>
<a name="l00733"></a>00733 <span class="comment"> * This function is often called repetitively to solve problems of identical</span>
<a name="l00734"></a>00734 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l00735"></a>00735 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l00736"></a>00736 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l00737"></a>00737 <span class="comment"> */</span>
<a name="l00738"></a>00738 <span class="keywordtype">int</span> sba_Axb_BK(<span class="keywordtype">double</span> *A, <span class="keywordtype">double</span> *B, <span class="keywordtype">double</span> *x, <span class="keywordtype">int</span> m, <span class="keywordtype">int</span> iscolmaj)
<a name="l00739"></a>00739 {
<a name="l00740"></a>00740 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l00741"></a>00741 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0, nb=0;
<a name="l00742"></a>00742 
<a name="l00743"></a>00743 <span class="keywordtype">int</span> a_sz, ipiv_sz, work_sz, tot_sz;
<a name="l00744"></a>00744 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00745"></a>00745 <span class="keywordtype">int</span> info, *ipiv, nrhs=1;
<a name="l00746"></a>00746 <span class="keywordtype">double</span> *a, *work;
<a name="l00747"></a>00747    
<a name="l00748"></a>00748     <span class="keywordflow">if</span>(A==NULL){
<a name="l00749"></a>00749       <span class="keywordflow">if</span>(buf) free(buf);
<a name="l00750"></a>00750       buf=NULL;
<a name="l00751"></a>00751       buf_sz=0;
<a name="l00752"></a>00752 
<a name="l00753"></a>00753       <span class="keywordflow">return</span> 1;
<a name="l00754"></a>00754     }
<a name="l00755"></a>00755 
<a name="l00756"></a>00756     <span class="comment">/* calculate required memory size */</span>
<a name="l00757"></a>00757     ipiv_sz=m;
<a name="l00758"></a>00758     a_sz=(iscolmaj)? 0 : m*m;
<a name="l00759"></a>00759     <span class="keywordflow">if</span>(!nb){
<a name="l00760"></a>00760 <span class="preprocessor">#ifndef SBA_LS_SCARCE_MEMORY</span>
<a name="l00761"></a>00761 <span class="preprocessor"></span>      <span class="keywordtype">double</span> tmp;
<a name="l00762"></a>00762 
<a name="l00763"></a>00763       work_sz=-1; <span class="comment">// workspace query; optimal size is returned in tmp</span>
<a name="l00764"></a>00764       F77_FUNC(dsytrf)(<span class="stringliteral">&quot;U&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">double</span> *)&amp;tmp, (<span class="keywordtype">int</span> *)&amp;work_sz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00765"></a>00765       nb=((int)tmp)/m; <span class="comment">// optimal worksize is m*nb</span>
<a name="l00766"></a>00766 <span class="preprocessor">#else</span>
<a name="l00767"></a>00767 <span class="preprocessor"></span>      nb=-1; <span class="comment">// min worksize is 1</span>
<a name="l00768"></a>00768 <span class="preprocessor">#endif </span><span class="comment">/* SBA_LS_SCARCE_MEMORY */</span>
<a name="l00769"></a>00769     }
<a name="l00770"></a>00770     work_sz=(nb!=-1)? nb*m : 1;
<a name="l00771"></a>00771     tot_sz=(a_sz + work_sz)*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>) + ipiv_sz*<span class="keyword">sizeof</span>(int); <span class="comment">/* should be arranged in that order for proper doubles alignment */</span>
<a name="l00772"></a>00772 
<a name="l00773"></a>00773     <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l00774"></a>00774       <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l00775"></a>00775 
<a name="l00776"></a>00776       buf_sz=tot_sz;
<a name="l00777"></a>00777       buf=(<span class="keywordtype">double</span> *)malloc(buf_sz);
<a name="l00778"></a>00778       <span class="keywordflow">if</span>(!buf){
<a name="l00779"></a>00779         fprintf(stderr, <span class="stringliteral">&quot;memory allocation in sba_Axb_BK() failed!\n&quot;</span>);
<a name="l00780"></a>00780         exit(1);
<a name="l00781"></a>00781       }
<a name="l00782"></a>00782     }
<a name="l00783"></a>00783 
<a name="l00784"></a>00784     <span class="keywordflow">if</span>(!iscolmaj){
<a name="l00785"></a>00785       a=buf;
<a name="l00786"></a>00786         work=a+a_sz;
<a name="l00787"></a>00787 
<a name="l00788"></a>00788       <span class="comment">/* store A into a and B into x; A is assumed to be symmetric, hence</span>
<a name="l00789"></a>00789 <span class="comment">       * the column and row major order representations are the same</span>
<a name="l00790"></a>00790 <span class="comment">       */</span>
<a name="l00791"></a>00791       <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){
<a name="l00792"></a>00792         a[i]=A[i];
<a name="l00793"></a>00793         x[i]=B[i];
<a name="l00794"></a>00794       }
<a name="l00795"></a>00795       <span class="keywordflow">for</span>(j=m*m; i&lt;j; ++i) <span class="comment">// copy remaining rows; note that i is not re-initialized</span>
<a name="l00796"></a>00796         a[i]=A[i];
<a name="l00797"></a>00797     }
<a name="l00798"></a>00798     <span class="keywordflow">else</span>{ <span class="comment">/* no copying is necessary for A */</span>
<a name="l00799"></a>00799       a=A;
<a name="l00800"></a>00800       <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l00801"></a>00801         x[i]=B[i];
<a name="l00802"></a>00802       work=buf;
<a name="l00803"></a>00803     }
<a name="l00804"></a>00804     ipiv=(<span class="keywordtype">int</span> *)(work+work_sz);
<a name="l00805"></a>00805 
<a name="l00806"></a>00806   <span class="comment">/* factorization for A */</span>
<a name="l00807"></a>00807         F77_FUNC(dsytrf)(<span class="stringliteral">&quot;U&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, ipiv, work, (<span class="keywordtype">int</span> *)&amp;work_sz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00808"></a>00808         <span class="keywordflow">if</span>(info!=0){
<a name="l00809"></a>00809                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l00810"></a>00810                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dsytrf illegal in sba_Axb_BK()\n&quot;</span>, -info);
<a name="l00811"></a>00811                         exit(1);
<a name="l00812"></a>00812                 }
<a name="l00813"></a>00813                 <span class="keywordflow">else</span>{
<a name="l00814"></a>00814                         fprintf(stderr, <span class="stringliteral">&quot;singular block diagonal matrix D for dsytrf in sba_Axb_BK() [D(%d, %d) is zero]\n&quot;</span>, info, info);
<a name="l00815"></a>00815                         <span class="keywordflow">return</span> 0;
<a name="l00816"></a>00816                 }
<a name="l00817"></a>00817         }
<a name="l00818"></a>00818 
<a name="l00819"></a>00819   <span class="comment">/* solve the system with the computed factorization */</span>
<a name="l00820"></a>00820   F77_FUNC(dsytrs)(<span class="stringliteral">&quot;U&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;nrhs, a, (<span class="keywordtype">int</span> *)&amp;m, ipiv, x, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00821"></a>00821         <span class="keywordflow">if</span>(info!=0){
<a name="l00822"></a>00822                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l00823"></a>00823                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dsytrs illegal in sba_Axb_BK()\n&quot;</span>, -info);
<a name="l00824"></a>00824                         exit(1);
<a name="l00825"></a>00825                 }
<a name="l00826"></a>00826                 <span class="keywordflow">else</span>{
<a name="l00827"></a>00827                         fprintf(stderr, <span class="stringliteral">&quot;unknown error for dsytrs in sba_Axb_BK()\n&quot;</span>);
<a name="l00828"></a>00828                         <span class="keywordflow">return</span> 0;
<a name="l00829"></a>00829                 }
<a name="l00830"></a>00830         }
<a name="l00831"></a>00831 
<a name="l00832"></a>00832         <span class="keywordflow">return</span> 1;
<a name="l00833"></a>00833 }
<a name="l00834"></a>00834 
<a name="l00835"></a>00835 <span class="comment">/*</span>
<a name="l00836"></a>00836 <span class="comment"> * This function computes the inverse of a square matrix whose upper triangle</span>
<a name="l00837"></a>00837 <span class="comment"> * is stored in A into its lower triangle using LU decomposition</span>
<a name="l00838"></a>00838 <span class="comment"> *</span>
<a name="l00839"></a>00839 <span class="comment"> * The function returns 0 in case of error (e.g. A is singular),</span>
<a name="l00840"></a>00840 <span class="comment"> * 1 if successfull</span>
<a name="l00841"></a>00841 <span class="comment"> *</span>
<a name="l00842"></a>00842 <span class="comment"> * This function is often called repetitively to solve problems of identical</span>
<a name="l00843"></a>00843 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l00844"></a>00844 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l00845"></a>00845 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l00846"></a>00846 <span class="comment"> */</span>
<a name="l00847"></a>00847 <span class="keywordtype">int</span> sba_symat_invert_LU(<span class="keywordtype">double</span> *A, <span class="keywordtype">int</span> m)
<a name="l00848"></a>00848 {
<a name="l00849"></a>00849 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l00850"></a>00850 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0, nb=0;
<a name="l00851"></a>00851 
<a name="l00852"></a>00852 <span class="keywordtype">int</span> a_sz, ipiv_sz, work_sz, tot_sz;
<a name="l00853"></a>00853 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00854"></a>00854 <span class="keywordtype">int</span> info, *ipiv;
<a name="l00855"></a>00855 <span class="keywordtype">double</span> *a, *work;
<a name="l00856"></a>00856    
<a name="l00857"></a>00857   <span class="keywordflow">if</span>(A==NULL){
<a name="l00858"></a>00858     <span class="keywordflow">if</span>(buf) free(buf);
<a name="l00859"></a>00859     buf=NULL;
<a name="l00860"></a>00860     buf_sz=0;
<a name="l00861"></a>00861 
<a name="l00862"></a>00862     <span class="keywordflow">return</span> 1;
<a name="l00863"></a>00863   }
<a name="l00864"></a>00864 
<a name="l00865"></a>00865   <span class="comment">/* calculate required memory size */</span>
<a name="l00866"></a>00866   ipiv_sz=m;
<a name="l00867"></a>00867   a_sz=m*m;
<a name="l00868"></a>00868   <span class="keywordflow">if</span>(!nb){
<a name="l00869"></a>00869 <span class="preprocessor">#ifndef SBA_LS_SCARCE_MEMORY</span>
<a name="l00870"></a>00870 <span class="preprocessor"></span>    <span class="keywordtype">double</span> tmp;
<a name="l00871"></a>00871 
<a name="l00872"></a>00872     work_sz=-1; <span class="comment">// workspace query; optimal size is returned in tmp</span>
<a name="l00873"></a>00873     F77_FUNC(dgetri)((<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">double</span> *)&amp;tmp, (<span class="keywordtype">int</span> *)&amp;work_sz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00874"></a>00874     nb=((int)tmp)/m; <span class="comment">// optimal worksize is m*nb</span>
<a name="l00875"></a>00875 <span class="preprocessor">#else</span>
<a name="l00876"></a>00876 <span class="preprocessor"></span>    nb=1; <span class="comment">// min worksize is m</span>
<a name="l00877"></a>00877 <span class="preprocessor">#endif </span><span class="comment">/* SBA_LS_SCARCE_MEMORY */</span>
<a name="l00878"></a>00878   }
<a name="l00879"></a>00879   work_sz=nb*m;
<a name="l00880"></a>00880   tot_sz=(a_sz + work_sz)*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>) + ipiv_sz*<span class="keyword">sizeof</span>(int); <span class="comment">/* should be arranged in that order for proper doubles alignment */</span>
<a name="l00881"></a>00881 
<a name="l00882"></a>00882   <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l00883"></a>00883     <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l00884"></a>00884 
<a name="l00885"></a>00885     buf_sz=tot_sz;
<a name="l00886"></a>00886     buf=(<span class="keywordtype">double</span> *)malloc(buf_sz);
<a name="l00887"></a>00887     <span class="keywordflow">if</span>(!buf){
<a name="l00888"></a>00888       fprintf(stderr, <span class="stringliteral">&quot;memory allocation in sba_symat_invert_LU() failed!\n&quot;</span>);
<a name="l00889"></a>00889       exit(1);
<a name="l00890"></a>00890     }
<a name="l00891"></a>00891   }
<a name="l00892"></a>00892 
<a name="l00893"></a>00893   a=buf;
<a name="l00894"></a>00894   work=a+a_sz;
<a name="l00895"></a>00895   ipiv=(<span class="keywordtype">int</span> *)(work+work_sz);
<a name="l00896"></a>00896 
<a name="l00897"></a>00897   <span class="comment">/* store A (column major!) into a */</span>
<a name="l00898"></a>00898         <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l00899"></a>00899                 <span class="keywordflow">for</span>(j=i; j&lt;m; ++j)
<a name="l00900"></a>00900                         a[i+j*m]=a[j+i*m]=A[i*m+j]; <span class="comment">// copy A&#39;s upper part to a&#39;s upper &amp; lower</span>
<a name="l00901"></a>00901 
<a name="l00902"></a>00902   <span class="comment">/* LU decomposition for A */</span>
<a name="l00903"></a>00903         F77_FUNC(dgetrf)((<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, ipiv, (<span class="keywordtype">int</span> *)&amp;info);  
<a name="l00904"></a>00904         <span class="keywordflow">if</span>(info!=0){
<a name="l00905"></a>00905                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l00906"></a>00906                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dgetrf illegal in sba_symat_invert_LU()\n&quot;</span>, -info);
<a name="l00907"></a>00907                         exit(1);
<a name="l00908"></a>00908                 }
<a name="l00909"></a>00909                 <span class="keywordflow">else</span>{
<a name="l00910"></a>00910                         fprintf(stderr, <span class="stringliteral">&quot;singular matrix A for dgetrf in sba_symat_invert_LU()\n&quot;</span>);
<a name="l00911"></a>00911                         <span class="keywordflow">return</span> 0;
<a name="l00912"></a>00912                 }
<a name="l00913"></a>00913         }
<a name="l00914"></a>00914 
<a name="l00915"></a>00915   <span class="comment">/* (A)^{-1} from LU */</span>
<a name="l00916"></a>00916         F77_FUNC(dgetri)((<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, ipiv, work, (<span class="keywordtype">int</span> *)&amp;work_sz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00917"></a>00917         <span class="keywordflow">if</span>(info!=0){
<a name="l00918"></a>00918                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l00919"></a>00919                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dgetri illegal in sba_symat_invert_LU()\n&quot;</span>, -info);
<a name="l00920"></a>00920                         exit(1);
<a name="l00921"></a>00921                 }
<a name="l00922"></a>00922                 <span class="keywordflow">else</span>{
<a name="l00923"></a>00923                         fprintf(stderr, <span class="stringliteral">&quot;singular matrix A for dgetri in sba_symat_invert_LU()\n&quot;</span>);
<a name="l00924"></a>00924                         <span class="keywordflow">return</span> 0;
<a name="l00925"></a>00925                 }
<a name="l00926"></a>00926         }
<a name="l00927"></a>00927 
<a name="l00928"></a>00928         <span class="comment">/* store (A)^{-1} in A&#39;s lower triangle */</span>
<a name="l00929"></a>00929         <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l00930"></a>00930                 <span class="keywordflow">for</span>(j=0; j&lt;=i; ++j)
<a name="l00931"></a>00931       A[i*m+j]=a[i+j*m];
<a name="l00932"></a>00932 
<a name="l00933"></a>00933         <span class="keywordflow">return</span> 1;
<a name="l00934"></a>00934 }
<a name="l00935"></a>00935 
<a name="l00936"></a>00936 <span class="comment">/*</span>
<a name="l00937"></a>00937 <span class="comment"> * This function computes the inverse of a square symmetric positive definite </span>
<a name="l00938"></a>00938 <span class="comment"> * matrix whose upper triangle is stored in A into its lower triangle using</span>
<a name="l00939"></a>00939 <span class="comment"> * Cholesky factorization</span>
<a name="l00940"></a>00940 <span class="comment"> *</span>
<a name="l00941"></a>00941 <span class="comment"> * The function returns 0 in case of error (e.g. A is not positive definite or singular),</span>
<a name="l00942"></a>00942 <span class="comment"> * 1 if successfull</span>
<a name="l00943"></a>00943 <span class="comment"> *</span>
<a name="l00944"></a>00944 <span class="comment"> * This function is often called repetitively to solve problems of identical</span>
<a name="l00945"></a>00945 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l00946"></a>00946 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l00947"></a>00947 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l00948"></a>00948 <span class="comment"> */</span>
<a name="l00949"></a>00949 <span class="keywordtype">int</span> sba_symat_invert_Chol(<span class="keywordtype">double</span> *A, <span class="keywordtype">int</span> m)
<a name="l00950"></a>00950 {
<a name="l00951"></a>00951 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l00952"></a>00952 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0;
<a name="l00953"></a>00953 
<a name="l00954"></a>00954 <span class="keywordtype">int</span> a_sz, tot_sz;
<a name="l00955"></a>00955 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00956"></a>00956 <span class="keywordtype">int</span> info;
<a name="l00957"></a>00957 <span class="keywordtype">double</span> *a;
<a name="l00958"></a>00958    
<a name="l00959"></a>00959   <span class="keywordflow">if</span>(A==NULL){
<a name="l00960"></a>00960     <span class="keywordflow">if</span>(buf) free(buf);
<a name="l00961"></a>00961     buf=NULL;
<a name="l00962"></a>00962     buf_sz=0;
<a name="l00963"></a>00963 
<a name="l00964"></a>00964     <span class="keywordflow">return</span> 1;
<a name="l00965"></a>00965   }
<a name="l00966"></a>00966 
<a name="l00967"></a>00967   <span class="comment">/* calculate required memory size */</span>
<a name="l00968"></a>00968   a_sz=m*m;
<a name="l00969"></a>00969   tot_sz=a_sz; 
<a name="l00970"></a>00970 
<a name="l00971"></a>00971   <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l00972"></a>00972     <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l00973"></a>00973 
<a name="l00974"></a>00974     buf_sz=tot_sz;
<a name="l00975"></a>00975     buf=(<span class="keywordtype">double</span> *)malloc(buf_sz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l00976"></a>00976     <span class="keywordflow">if</span>(!buf){
<a name="l00977"></a>00977       fprintf(stderr, <span class="stringliteral">&quot;memory allocation in sba_symat_invert_Chol() failed!\n&quot;</span>);
<a name="l00978"></a>00978       exit(1);
<a name="l00979"></a>00979     }
<a name="l00980"></a>00980   }
<a name="l00981"></a>00981 
<a name="l00982"></a>00982   a=(<span class="keywordtype">double</span> *)buf;
<a name="l00983"></a>00983 
<a name="l00984"></a>00984   <span class="comment">/* store A into a; A is assumed symmetric, hence no transposition is needed */</span>
<a name="l00985"></a>00985   <span class="keywordflow">for</span>(i=0, j=a_sz; i&lt;j; ++i)
<a name="l00986"></a>00986     a[i]=A[i];
<a name="l00987"></a>00987 
<a name="l00988"></a>00988   <span class="comment">/* Cholesky factorization for A; a&#39;s lower part corresponds to A&#39;s upper */</span>
<a name="l00989"></a>00989   <span class="comment">//F77_FUNC(dpotrf)(&quot;L&quot;, (int *)&amp;m, a, (int *)&amp;m, (int *)&amp;info);</span>
<a name="l00990"></a>00990   F77_FUNC(dpotf2)(<span class="stringliteral">&quot;L&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l00991"></a>00991   <span class="comment">/* error treatment */</span>
<a name="l00992"></a>00992   <span class="keywordflow">if</span>(info!=0){
<a name="l00993"></a>00993     <span class="keywordflow">if</span>(info&lt;0){
<a name="l00994"></a>00994       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dpotrf in sba_symat_invert_Chol()\n&quot;</span>, -info);
<a name="l00995"></a>00995       exit(1);
<a name="l00996"></a>00996     }
<a name="l00997"></a>00997     <span class="keywordflow">else</span>{
<a name="l00998"></a>00998       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: the leading minor of order %d is not positive definite,\nthe factorization could not be completed for dpotrf in sba_symat_invert_Chol()\n&quot;</span>, info);
<a name="l00999"></a>00999       <span class="keywordflow">return</span> 0;
<a name="l01000"></a>01000     }
<a name="l01001"></a>01001   }
<a name="l01002"></a>01002 
<a name="l01003"></a>01003   <span class="comment">/* (A)^{-1} from Cholesky */</span>
<a name="l01004"></a>01004   F77_FUNC(dpotri)(<span class="stringliteral">&quot;L&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l01005"></a>01005         <span class="keywordflow">if</span>(info!=0){
<a name="l01006"></a>01006                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l01007"></a>01007                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dpotri illegal in sba_symat_invert_Chol()\n&quot;</span>, -info);
<a name="l01008"></a>01008                         exit(1);
<a name="l01009"></a>01009                 }
<a name="l01010"></a>01010                 <span class="keywordflow">else</span>{
<a name="l01011"></a>01011                         fprintf(stderr, <span class="stringliteral">&quot;the (%d, %d) element of the factor U or L is zero, singular matrix A for dpotri in sba_symat_invert_Chol()\n&quot;</span>, info, info);
<a name="l01012"></a>01012                         <span class="keywordflow">return</span> 0;
<a name="l01013"></a>01013                 }
<a name="l01014"></a>01014         }
<a name="l01015"></a>01015 
<a name="l01016"></a>01016         <span class="comment">/* store (A)^{-1} in A&#39;s lower triangle. The lower triangle of the symmetric A^{-1} is in the lower triangle of a */</span>
<a name="l01017"></a>01017         <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l01018"></a>01018                 <span class="keywordflow">for</span>(j=0; j&lt;=i; ++j)
<a name="l01019"></a>01019       A[i*m+j]=a[i+j*m];
<a name="l01020"></a>01020 
<a name="l01021"></a>01021         <span class="keywordflow">return</span> 1;
<a name="l01022"></a>01022 }
<a name="l01023"></a>01023 
<a name="l01024"></a>01024 <span class="comment">/*</span>
<a name="l01025"></a>01025 <span class="comment"> * This function computes the inverse of a symmetric indefinite </span>
<a name="l01026"></a>01026 <span class="comment"> * matrix whose upper triangle is stored in A into its lower triangle</span>
<a name="l01027"></a>01027 <span class="comment"> * using LDLT factorization with the pivoting strategy of Bunch and Kaufman</span>
<a name="l01028"></a>01028 <span class="comment"> *</span>
<a name="l01029"></a>01029 <span class="comment"> * The function returns 0 in case of error (e.g. A is singular),</span>
<a name="l01030"></a>01030 <span class="comment"> * 1 if successfull</span>
<a name="l01031"></a>01031 <span class="comment"> *</span>
<a name="l01032"></a>01032 <span class="comment"> * This function is often called repetitively to solve problems of identical</span>
<a name="l01033"></a>01033 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l01034"></a>01034 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l01035"></a>01035 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l01036"></a>01036 <span class="comment"> */</span>
<a name="l01037"></a>01037 <span class="keywordtype">int</span> sba_symat_invert_BK(<span class="keywordtype">double</span> *A, <span class="keywordtype">int</span> m)
<a name="l01038"></a>01038 {
<a name="l01039"></a>01039 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l01040"></a>01040 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0, nb=0;
<a name="l01041"></a>01041 
<a name="l01042"></a>01042 <span class="keywordtype">int</span> a_sz, ipiv_sz, work_sz, tot_sz;
<a name="l01043"></a>01043 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l01044"></a>01044 <span class="keywordtype">int</span> info, *ipiv;
<a name="l01045"></a>01045 <span class="keywordtype">double</span> *a, *work;
<a name="l01046"></a>01046    
<a name="l01047"></a>01047   <span class="keywordflow">if</span>(A==NULL){
<a name="l01048"></a>01048     <span class="keywordflow">if</span>(buf) free(buf);
<a name="l01049"></a>01049     buf=NULL;
<a name="l01050"></a>01050     buf_sz=0;
<a name="l01051"></a>01051 
<a name="l01052"></a>01052     <span class="keywordflow">return</span> 1;
<a name="l01053"></a>01053   }
<a name="l01054"></a>01054 
<a name="l01055"></a>01055   <span class="comment">/* calculate required memory size */</span>
<a name="l01056"></a>01056   ipiv_sz=m;
<a name="l01057"></a>01057   a_sz=m*m;
<a name="l01058"></a>01058   <span class="keywordflow">if</span>(!nb){
<a name="l01059"></a>01059 <span class="preprocessor">#ifndef SBA_LS_SCARCE_MEMORY</span>
<a name="l01060"></a>01060 <span class="preprocessor"></span>    <span class="keywordtype">double</span> tmp;
<a name="l01061"></a>01061 
<a name="l01062"></a>01062     work_sz=-1; <span class="comment">// workspace query; optimal size is returned in tmp</span>
<a name="l01063"></a>01063     F77_FUNC(dsytrf)(<span class="stringliteral">&quot;L&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">double</span> *)&amp;tmp, (<span class="keywordtype">int</span> *)&amp;work_sz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l01064"></a>01064     nb=((int)tmp)/m; <span class="comment">// optimal worksize is m*nb</span>
<a name="l01065"></a>01065 <span class="preprocessor">#else</span>
<a name="l01066"></a>01066 <span class="preprocessor"></span>    nb=-1; <span class="comment">// min worksize is 1</span>
<a name="l01067"></a>01067 <span class="preprocessor">#endif </span><span class="comment">/* SBA_LS_SCARCE_MEMORY */</span>
<a name="l01068"></a>01068   }
<a name="l01069"></a>01069   work_sz=(nb!=-1)? nb*m : 1;
<a name="l01070"></a>01070   work_sz=(work_sz&gt;=m)? work_sz : m; <span class="comment">/* ensure that work is at least m elements long, as required by dsytri */</span>
<a name="l01071"></a>01071   tot_sz=(a_sz + work_sz)*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>) + ipiv_sz*<span class="keyword">sizeof</span>(int); <span class="comment">/* should be arranged in that order for proper doubles alignment */</span>
<a name="l01072"></a>01072 
<a name="l01073"></a>01073   <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l01074"></a>01074     <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l01075"></a>01075 
<a name="l01076"></a>01076     buf_sz=tot_sz;
<a name="l01077"></a>01077     buf=(<span class="keywordtype">double</span> *)malloc(buf_sz);
<a name="l01078"></a>01078     <span class="keywordflow">if</span>(!buf){
<a name="l01079"></a>01079       fprintf(stderr, <span class="stringliteral">&quot;memory allocation in sba_symat_invert_BK() failed!\n&quot;</span>);
<a name="l01080"></a>01080       exit(1);
<a name="l01081"></a>01081     }
<a name="l01082"></a>01082   }
<a name="l01083"></a>01083 
<a name="l01084"></a>01084   a=buf;
<a name="l01085"></a>01085   work=a+a_sz;
<a name="l01086"></a>01086   ipiv=(<span class="keywordtype">int</span> *)(work+work_sz);
<a name="l01087"></a>01087 
<a name="l01088"></a>01088   <span class="comment">/* store A into a; A is assumed symmetric, hence no transposition is needed */</span>
<a name="l01089"></a>01089   <span class="keywordflow">for</span>(i=0, j=a_sz; i&lt;j; ++i)
<a name="l01090"></a>01090     a[i]=A[i];
<a name="l01091"></a>01091 
<a name="l01092"></a>01092   <span class="comment">/* LDLT factorization for A; a&#39;s lower part corresponds to A&#39;s upper */</span>
<a name="l01093"></a>01093         F77_FUNC(dsytrf)(<span class="stringliteral">&quot;L&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, ipiv, work, (<span class="keywordtype">int</span> *)&amp;work_sz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l01094"></a>01094         <span class="keywordflow">if</span>(info!=0){
<a name="l01095"></a>01095                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l01096"></a>01096                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dsytrf illegal in sba_symat_invert_BK()\n&quot;</span>, -info);
<a name="l01097"></a>01097                         exit(1);
<a name="l01098"></a>01098                 }
<a name="l01099"></a>01099                 <span class="keywordflow">else</span>{
<a name="l01100"></a>01100                         fprintf(stderr, <span class="stringliteral">&quot;singular block diagonal matrix D for dsytrf in sba_symat_invert_BK() [D(%d, %d) is zero]\n&quot;</span>, info, info);
<a name="l01101"></a>01101                         <span class="keywordflow">return</span> 0;
<a name="l01102"></a>01102                 }
<a name="l01103"></a>01103         }
<a name="l01104"></a>01104 
<a name="l01105"></a>01105   <span class="comment">/* (A)^{-1} from LDLT */</span>
<a name="l01106"></a>01106   F77_FUNC(dsytri)(<span class="stringliteral">&quot;L&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, ipiv, work, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l01107"></a>01107         <span class="keywordflow">if</span>(info!=0){
<a name="l01108"></a>01108                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l01109"></a>01109                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dsytri illegal in sba_symat_invert_BK()\n&quot;</span>, -info);
<a name="l01110"></a>01110                         exit(1);
<a name="l01111"></a>01111                 }
<a name="l01112"></a>01112                 <span class="keywordflow">else</span>{
<a name="l01113"></a>01113                         fprintf(stderr, <span class="stringliteral">&quot;D(%d, %d)=0, matrix is singular and its inverse could not be computed in sba_symat_invert_BK()\n&quot;</span>, info, info);
<a name="l01114"></a>01114                         <span class="keywordflow">return</span> 0;
<a name="l01115"></a>01115                 }
<a name="l01116"></a>01116         }
<a name="l01117"></a>01117 
<a name="l01118"></a>01118         <span class="comment">/* store (A)^{-1} in A&#39;s lower triangle. The lower triangle of the symmetric A^{-1} is in the lower triangle of a */</span>
<a name="l01119"></a>01119         <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l01120"></a>01120                 <span class="keywordflow">for</span>(j=0; j&lt;=i; ++j)
<a name="l01121"></a>01121       A[i*m+j]=a[i+j*m];
<a name="l01122"></a>01122 
<a name="l01123"></a>01123         <span class="keywordflow">return</span> 1;
<a name="l01124"></a>01124 }
<a name="l01125"></a>01125 
<a name="l01126"></a>01126 
<a name="l01127"></a>01127 <span class="preprocessor">#define __CG_LINALG_BLOCKSIZE           8</span>
<a name="l01128"></a>01128 <span class="preprocessor"></span>
<a name="l01129"></a>01129 <span class="comment">/* Dot product of two vectors x and y using loop unrolling and blocking.</span>
<a name="l01130"></a>01130 <span class="comment"> * see http://www.abarnett.demon.co.uk/tutorial.html</span>
<a name="l01131"></a>01131 <span class="comment"> */</span>
<a name="l01132"></a>01132 
<a name="l01133"></a>01133 <span class="keyword">inline</span> <span class="keyword">static</span> <span class="keywordtype">double</span> dprod(<span class="keyword">const</span> <span class="keywordtype">int</span> n, <span class="keyword">const</span> <span class="keywordtype">double</span> *<span class="keyword">const</span> x, <span class="keyword">const</span> <span class="keywordtype">double</span> *<span class="keyword">const</span> y)
<a name="l01134"></a>01134 {
<a name="l01135"></a>01135 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j1, j2, j3, j4, j5, j6, j7; 
<a name="l01136"></a>01136 <span class="keywordtype">int</span> blockn;
<a name="l01137"></a>01137 <span class="keyword">register</span> <span class="keywordtype">double</span> sum0=0.0, sum1=0.0, sum2=0.0, sum3=0.0,
<a name="l01138"></a>01138                 sum4=0.0, sum5=0.0, sum6=0.0, sum7=0.0;
<a name="l01139"></a>01139 
<a name="l01140"></a>01140   <span class="comment">/* n may not be divisible by __CG_LINALG_BLOCKSIZE, </span>
<a name="l01141"></a>01141 <span class="comment">  * go as near as we can first, then tidy up.</span>
<a name="l01142"></a>01142 <span class="comment">  */</span> 
<a name="l01143"></a>01143   blockn = (n / __CG_LINALG_BLOCKSIZE) * __CG_LINALG_BLOCKSIZE; 
<a name="l01144"></a>01144 
<a name="l01145"></a>01145   <span class="comment">/* unroll the loop in blocks of `__CG_LINALG_BLOCKSIZE&#39; */</span> 
<a name="l01146"></a>01146   <span class="keywordflow">for</span>(i=0; i&lt;blockn; i+=__CG_LINALG_BLOCKSIZE){
<a name="l01147"></a>01147             sum0+=x[i]*y[i];
<a name="l01148"></a>01148     j1=i+1; sum1+=x[j1]*y[j1];
<a name="l01149"></a>01149     j2=i+2; sum2+=x[j2]*y[j2];
<a name="l01150"></a>01150     j3=i+3; sum3+=x[j3]*y[j3];
<a name="l01151"></a>01151     j4=i+4; sum4+=x[j4]*y[j4];
<a name="l01152"></a>01152     j5=i+5; sum5+=x[j5]*y[j5];
<a name="l01153"></a>01153     j6=i+6; sum6+=x[j6]*y[j6];
<a name="l01154"></a>01154     j7=i+7; sum7+=x[j7]*y[j7];
<a name="l01155"></a>01155   } 
<a name="l01156"></a>01156 
<a name="l01157"></a>01157  <span class="comment">/* </span>
<a name="l01158"></a>01158 <span class="comment">  * There may be some left to do.</span>
<a name="l01159"></a>01159 <span class="comment">  * This could be done as a simple for() loop, </span>
<a name="l01160"></a>01160 <span class="comment">  * but a switch is faster (and more interesting) </span>
<a name="l01161"></a>01161 <span class="comment">  */</span> 
<a name="l01162"></a>01162 
<a name="l01163"></a>01163   <span class="keywordflow">if</span>(i&lt;n){ 
<a name="l01164"></a>01164     <span class="comment">/* Jump into the case at the place that will allow</span>
<a name="l01165"></a>01165 <span class="comment">    * us to finish off the appropriate number of items. </span>
<a name="l01166"></a>01166 <span class="comment">    */</span> 
<a name="l01167"></a>01167 
<a name="l01168"></a>01168     <span class="keywordflow">switch</span>(n - i){ 
<a name="l01169"></a>01169       <span class="keywordflow">case</span> 7 : sum0+=x[i]*y[i]; ++i;
<a name="l01170"></a>01170       <span class="keywordflow">case</span> 6 : sum1+=x[i]*y[i]; ++i;
<a name="l01171"></a>01171       <span class="keywordflow">case</span> 5 : sum2+=x[i]*y[i]; ++i;
<a name="l01172"></a>01172       <span class="keywordflow">case</span> 4 : sum3+=x[i]*y[i]; ++i;
<a name="l01173"></a>01173       <span class="keywordflow">case</span> 3 : sum4+=x[i]*y[i]; ++i;
<a name="l01174"></a>01174       <span class="keywordflow">case</span> 2 : sum5+=x[i]*y[i]; ++i;
<a name="l01175"></a>01175       <span class="keywordflow">case</span> 1 : sum6+=x[i]*y[i]; ++i;
<a name="l01176"></a>01176     }
<a name="l01177"></a>01177   } 
<a name="l01178"></a>01178 
<a name="l01179"></a>01179   <span class="keywordflow">return</span> sum0+sum1+sum2+sum3+sum4+sum5+sum6+sum7;
<a name="l01180"></a>01180 }
<a name="l01181"></a>01181 
<a name="l01182"></a>01182 
<a name="l01183"></a>01183 <span class="comment">/* Compute z=x+a*y for two vectors x and y and a scalar a; z can be one of x, y.</span>
<a name="l01184"></a>01184 <span class="comment"> * Similarly to the dot product routine, this one uses loop unrolling and blocking</span>
<a name="l01185"></a>01185 <span class="comment"> */</span>
<a name="l01186"></a>01186 
<a name="l01187"></a>01187 <span class="keyword">inline</span> <span class="keyword">static</span> <span class="keywordtype">void</span> daxpy(<span class="keyword">const</span> <span class="keywordtype">int</span> n, <span class="keywordtype">double</span> *<span class="keyword">const</span> z, <span class="keyword">const</span> <span class="keywordtype">double</span> *<span class="keyword">const</span> x, <span class="keyword">const</span> <span class="keywordtype">double</span> a, <span class="keyword">const</span> <span class="keywordtype">double</span> *<span class="keyword">const</span> y)
<a name="l01188"></a>01188 { 
<a name="l01189"></a>01189 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j1, j2, j3, j4, j5, j6, j7; 
<a name="l01190"></a>01190 <span class="keywordtype">int</span> blockn;
<a name="l01191"></a>01191 
<a name="l01192"></a>01192   <span class="comment">/* n may not be divisible by __CG_LINALG_BLOCKSIZE, </span>
<a name="l01193"></a>01193 <span class="comment">  * go as near as we can first, then tidy up.</span>
<a name="l01194"></a>01194 <span class="comment">  */</span> 
<a name="l01195"></a>01195   blockn = (n / __CG_LINALG_BLOCKSIZE) * __CG_LINALG_BLOCKSIZE; 
<a name="l01196"></a>01196 
<a name="l01197"></a>01197   <span class="comment">/* unroll the loop in blocks of `__CG_LINALG_BLOCKSIZE&#39; */</span> 
<a name="l01198"></a>01198   <span class="keywordflow">for</span>(i=0; i&lt;blockn; i+=__CG_LINALG_BLOCKSIZE){
<a name="l01199"></a>01199             z[i]=x[i]+a*y[i];
<a name="l01200"></a>01200     j1=i+1; z[j1]=x[j1]+a*y[j1];
<a name="l01201"></a>01201     j2=i+2; z[j2]=x[j2]+a*y[j2];
<a name="l01202"></a>01202     j3=i+3; z[j3]=x[j3]+a*y[j3];
<a name="l01203"></a>01203     j4=i+4; z[j4]=x[j4]+a*y[j4];
<a name="l01204"></a>01204     j5=i+5; z[j5]=x[j5]+a*y[j5];
<a name="l01205"></a>01205     j6=i+6; z[j6]=x[j6]+a*y[j6];
<a name="l01206"></a>01206     j7=i+7; z[j7]=x[j7]+a*y[j7];
<a name="l01207"></a>01207   } 
<a name="l01208"></a>01208 
<a name="l01209"></a>01209  <span class="comment">/* </span>
<a name="l01210"></a>01210 <span class="comment">  * There may be some left to do.</span>
<a name="l01211"></a>01211 <span class="comment">  * This could be done as a simple for() loop, </span>
<a name="l01212"></a>01212 <span class="comment">  * but a switch is faster (and more interesting) </span>
<a name="l01213"></a>01213 <span class="comment">  */</span> 
<a name="l01214"></a>01214 
<a name="l01215"></a>01215   <span class="keywordflow">if</span>(i&lt;n){ 
<a name="l01216"></a>01216     <span class="comment">/* Jump into the case at the place that will allow</span>
<a name="l01217"></a>01217 <span class="comment">    * us to finish off the appropriate number of items. </span>
<a name="l01218"></a>01218 <span class="comment">    */</span> 
<a name="l01219"></a>01219 
<a name="l01220"></a>01220     <span class="keywordflow">switch</span>(n - i){ 
<a name="l01221"></a>01221       <span class="keywordflow">case</span> 7 : z[i]=x[i]+a*y[i]; ++i;
<a name="l01222"></a>01222       <span class="keywordflow">case</span> 6 : z[i]=x[i]+a*y[i]; ++i;
<a name="l01223"></a>01223       <span class="keywordflow">case</span> 5 : z[i]=x[i]+a*y[i]; ++i;
<a name="l01224"></a>01224       <span class="keywordflow">case</span> 4 : z[i]=x[i]+a*y[i]; ++i;
<a name="l01225"></a>01225       <span class="keywordflow">case</span> 3 : z[i]=x[i]+a*y[i]; ++i;
<a name="l01226"></a>01226       <span class="keywordflow">case</span> 2 : z[i]=x[i]+a*y[i]; ++i;
<a name="l01227"></a>01227       <span class="keywordflow">case</span> 1 : z[i]=x[i]+a*y[i]; ++i;
<a name="l01228"></a>01228     }
<a name="l01229"></a>01229   } 
<a name="l01230"></a>01230 }
<a name="l01231"></a>01231 
<a name="l01232"></a>01232 <span class="comment">/*</span>
<a name="l01233"></a>01233 <span class="comment"> * This function returns the solution of Ax = b where A is posititive definite,</span>
<a name="l01234"></a>01234 <span class="comment"> * based on the conjugate gradients method; see &quot;An intro to the CG method&quot; by J.R. Shewchuk, p. 50-51</span>
<a name="l01235"></a>01235 <span class="comment"> *</span>
<a name="l01236"></a>01236 <span class="comment"> * A is mxm, b, x are is mx1. Argument niter specifies the maximum number of </span>
<a name="l01237"></a>01237 <span class="comment"> * iterations and eps is the desired solution accuracy. niter&lt;0 signals that</span>
<a name="l01238"></a>01238 <span class="comment"> * x contains a valid initial approximation to the solution; if niter&gt;0 then </span>
<a name="l01239"></a>01239 <span class="comment"> * the starting point is taken to be zero. Argument prec selects the desired</span>
<a name="l01240"></a>01240 <span class="comment"> * preconditioning method as follows:</span>
<a name="l01241"></a>01241 <span class="comment"> * 0: no preconditioning</span>
<a name="l01242"></a>01242 <span class="comment"> * 1: jacobi (diagonal) preconditioning</span>
<a name="l01243"></a>01243 <span class="comment"> * 2: SSOR preconditioning</span>
<a name="l01244"></a>01244 <span class="comment"> * Argument iscolmaj specifies whether A is stored in column or row major order.</span>
<a name="l01245"></a>01245 <span class="comment"> *</span>
<a name="l01246"></a>01246 <span class="comment"> * The function returns 0 in case of error,</span>
<a name="l01247"></a>01247 <span class="comment"> * the number of iterations performed if successfull</span>
<a name="l01248"></a>01248 <span class="comment"> *</span>
<a name="l01249"></a>01249 <span class="comment"> * This function is often called repetitively to solve problems of identical</span>
<a name="l01250"></a>01250 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l01251"></a>01251 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l01252"></a>01252 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l01253"></a>01253 <span class="comment"> */</span>
<a name="l01254"></a>01254 <span class="keywordtype">int</span> sba_Axb_CG(<span class="keywordtype">double</span> *A, <span class="keywordtype">double</span> *B, <span class="keywordtype">double</span> *x, <span class="keywordtype">int</span> m, <span class="keywordtype">int</span> niter, <span class="keywordtype">double</span> eps, <span class="keywordtype">int</span> prec, <span class="keywordtype">int</span> iscolmaj)
<a name="l01255"></a>01255 {
<a name="l01256"></a>01256 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l01257"></a>01257 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0;
<a name="l01258"></a>01258 
<a name="l01259"></a>01259 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l01260"></a>01260 <span class="keyword">register</span> <span class="keywordtype">double</span> *aim;
<a name="l01261"></a>01261 <span class="keywordtype">int</span> iter, a_sz, res_sz, d_sz, q_sz, s_sz, wk_sz, z_sz, tot_sz;
<a name="l01262"></a>01262 <span class="keywordtype">double</span> *a, *res, *d, *q, *s, *wk, *z;
<a name="l01263"></a>01263 <span class="keywordtype">double</span> delta0, deltaold, deltanew, alpha, beta, eps_sq=eps*eps;
<a name="l01264"></a>01264 <span class="keyword">register</span> <span class="keywordtype">double</span> sum;
<a name="l01265"></a>01265 <span class="keywordtype">int</span> rec_res;
<a name="l01266"></a>01266 
<a name="l01267"></a>01267   <span class="keywordflow">if</span>(A==NULL){
<a name="l01268"></a>01268     <span class="keywordflow">if</span>(buf) free(buf);
<a name="l01269"></a>01269     buf=NULL;
<a name="l01270"></a>01270     buf_sz=0;
<a name="l01271"></a>01271 
<a name="l01272"></a>01272     <span class="keywordflow">return</span> 1;
<a name="l01273"></a>01273   }
<a name="l01274"></a>01274 
<a name="l01275"></a>01275   <span class="comment">/* calculate required memory size */</span>
<a name="l01276"></a>01276   a_sz=(iscolmaj)? m*m : 0;
<a name="l01277"></a>01277         res_sz=m; d_sz=m; q_sz=m;
<a name="l01278"></a>01278   <span class="keywordflow">if</span>(prec!=SBA_CG_NOPREC){
<a name="l01279"></a>01279     s_sz=m; wk_sz=m;
<a name="l01280"></a>01280     z_sz=(prec==SBA_CG_SSOR)? m : 0;
<a name="l01281"></a>01281   }
<a name="l01282"></a>01282   <span class="keywordflow">else</span>
<a name="l01283"></a>01283     s_sz=wk_sz=z_sz=0;
<a name="l01284"></a>01284  
<a name="l01285"></a>01285         tot_sz=a_sz+res_sz+d_sz+q_sz+s_sz+wk_sz+z_sz;
<a name="l01286"></a>01286 
<a name="l01287"></a>01287   <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l01288"></a>01288     <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l01289"></a>01289 
<a name="l01290"></a>01290     buf_sz=tot_sz;
<a name="l01291"></a>01291     buf=(<span class="keywordtype">double</span> *)malloc(buf_sz*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>));
<a name="l01292"></a>01292     <span class="keywordflow">if</span>(!buf){
<a name="l01293"></a>01293                   fprintf(stderr, <span class="stringliteral">&quot;memory allocation request failed in sba_Axb_CG()\n&quot;</span>);
<a name="l01294"></a>01294                   exit(1);
<a name="l01295"></a>01295           }
<a name="l01296"></a>01296   }
<a name="l01297"></a>01297 
<a name="l01298"></a>01298   <span class="keywordflow">if</span>(iscolmaj){ 
<a name="l01299"></a>01299     a=buf;
<a name="l01300"></a>01300     <span class="comment">/* store A (row major!) into a */</span>
<a name="l01301"></a>01301     <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l01302"></a>01302       <span class="keywordflow">for</span>(j=0, aim=a+i*m; j&lt;m; ++j)
<a name="l01303"></a>01303         aim[j]=A[i+j*m];
<a name="l01304"></a>01304   }
<a name="l01305"></a>01305   <span class="keywordflow">else</span> a=A; <span class="comment">/* no copying required */</span>
<a name="l01306"></a>01306 
<a name="l01307"></a>01307         res=buf+a_sz;
<a name="l01308"></a>01308         d=res+res_sz;
<a name="l01309"></a>01309         q=d+d_sz;
<a name="l01310"></a>01310   <span class="keywordflow">if</span>(prec!=SBA_CG_NOPREC){
<a name="l01311"></a>01311           s=q+q_sz;
<a name="l01312"></a>01312     wk=s+s_sz;
<a name="l01313"></a>01313     z=(prec==SBA_CG_SSOR)? wk+wk_sz : NULL;
<a name="l01314"></a>01314 
<a name="l01315"></a>01315     <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){ <span class="comment">// compute jacobi (i.e. diagonal) preconditioners and save them in wk</span>
<a name="l01316"></a>01316       sum=a[i*m+i];
<a name="l01317"></a>01317       <span class="keywordflow">if</span>(sum&gt;DBL_EPSILON || -sum&lt;-DBL_EPSILON) <span class="comment">// != 0.0</span>
<a name="l01318"></a>01318         wk[i]=1.0/sum;
<a name="l01319"></a>01319       <span class="keywordflow">else</span>
<a name="l01320"></a>01320         wk[i]=1.0/DBL_EPSILON;
<a name="l01321"></a>01321     }
<a name="l01322"></a>01322   }
<a name="l01323"></a>01323   <span class="keywordflow">else</span>{
<a name="l01324"></a>01324     s=res;
<a name="l01325"></a>01325     wk=z=NULL;
<a name="l01326"></a>01326   }
<a name="l01327"></a>01327 
<a name="l01328"></a>01328   <span class="keywordflow">if</span>(niter&gt;0){
<a name="l01329"></a>01329           <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){ <span class="comment">// clear solution and initialize residual vector:  res &lt;-- B</span>
<a name="l01330"></a>01330                   x[i]=0.0;
<a name="l01331"></a>01331       res[i]=B[i];
<a name="l01332"></a>01332     }
<a name="l01333"></a>01333   }
<a name="l01334"></a>01334   <span class="keywordflow">else</span>{
<a name="l01335"></a>01335     niter=-niter;
<a name="l01336"></a>01336 
<a name="l01337"></a>01337           <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){ <span class="comment">// initialize residual vector:  res &lt;-- B - A*x</span>
<a name="l01338"></a>01338       <span class="keywordflow">for</span>(j=0, aim=a+i*m, sum=0.0; j&lt;m; ++j)
<a name="l01339"></a>01339         sum+=aim[j]*x[j];
<a name="l01340"></a>01340       res[i]=B[i]-sum;
<a name="l01341"></a>01341     }
<a name="l01342"></a>01342   }
<a name="l01343"></a>01343 
<a name="l01344"></a>01344   <span class="keywordflow">switch</span>(prec){
<a name="l01345"></a>01345     <span class="keywordflow">case</span> SBA_CG_NOPREC:
<a name="l01346"></a>01346       <span class="keywordflow">for</span>(i=0, deltanew=0.0; i&lt;m; ++i){
<a name="l01347"></a>01347         d[i]=res[i];
<a name="l01348"></a>01348         deltanew+=res[i]*res[i];
<a name="l01349"></a>01349       }
<a name="l01350"></a>01350       <span class="keywordflow">break</span>;
<a name="l01351"></a>01351     <span class="keywordflow">case</span> SBA_CG_JACOBI: <span class="comment">// jacobi preconditioning</span>
<a name="l01352"></a>01352       <span class="keywordflow">for</span>(i=0, deltanew=0.0; i&lt;m; ++i){
<a name="l01353"></a>01353         d[i]=res[i]*wk[i];
<a name="l01354"></a>01354         deltanew+=res[i]*d[i];
<a name="l01355"></a>01355       }
<a name="l01356"></a>01356       <span class="keywordflow">break</span>;
<a name="l01357"></a>01357     <span class="keywordflow">case</span> SBA_CG_SSOR: <span class="comment">// SSOR preconditioning; see the &quot;templates&quot; book, fig. 3.2, p. 44</span>
<a name="l01358"></a>01358       <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){
<a name="l01359"></a>01359         <span class="keywordflow">for</span>(j=0, sum=0.0, aim=a+i*m; j&lt;i; ++j)
<a name="l01360"></a>01360           sum+=aim[j]*z[j];
<a name="l01361"></a>01361         z[i]=wk[i]*(res[i]-sum);
<a name="l01362"></a>01362       }
<a name="l01363"></a>01363 
<a name="l01364"></a>01364       <span class="keywordflow">for</span>(i=m-1; i&gt;=0; --i){
<a name="l01365"></a>01365         <span class="keywordflow">for</span>(j=i+1, sum=0.0, aim=a+i*m; j&lt;m; ++j)
<a name="l01366"></a>01366           sum+=aim[j]*d[j];
<a name="l01367"></a>01367         d[i]=z[i]-wk[i]*sum;
<a name="l01368"></a>01368       }
<a name="l01369"></a>01369       deltanew=dprod(m, res, d);
<a name="l01370"></a>01370       <span class="keywordflow">break</span>;
<a name="l01371"></a>01371     <span class="keywordflow">default</span>:
<a name="l01372"></a>01372       fprintf(stderr, <span class="stringliteral">&quot;unknown preconditioning option %d in sba_Axb_CG\n&quot;</span>, prec);
<a name="l01373"></a>01373       exit(1);
<a name="l01374"></a>01374   }
<a name="l01375"></a>01375 
<a name="l01376"></a>01376   delta0=deltanew;
<a name="l01377"></a>01377 
<a name="l01378"></a>01378         <span class="keywordflow">for</span>(iter=1; deltanew&gt;eps_sq*delta0 &amp;&amp; iter&lt;=niter; ++iter){
<a name="l01379"></a>01379     <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){ <span class="comment">// q &lt;-- A d</span>
<a name="l01380"></a>01380       aim=a+i*m;
<a name="l01381"></a>01381 <span class="comment">/***</span>
<a name="l01382"></a>01382 <span class="comment">      for(j=0, sum=0.0; j&lt;m; ++j)</span>
<a name="l01383"></a>01383 <span class="comment">        sum+=aim[j]*d[j];</span>
<a name="l01384"></a>01384 <span class="comment">***/</span>
<a name="l01385"></a>01385       q[i]=dprod(m, aim, d); <span class="comment">//sum;</span>
<a name="l01386"></a>01386     }
<a name="l01387"></a>01387 
<a name="l01388"></a>01388 <span class="comment">/***</span>
<a name="l01389"></a>01389 <span class="comment">    for(i=0, sum=0.0; i&lt;m; ++i)</span>
<a name="l01390"></a>01390 <span class="comment">      sum+=d[i]*q[i];</span>
<a name="l01391"></a>01391 <span class="comment">***/</span>
<a name="l01392"></a>01392     alpha=deltanew/dprod(m, d, q); <span class="comment">// deltanew/sum;</span>
<a name="l01393"></a>01393 
<a name="l01394"></a>01394 <span class="comment">/***</span>
<a name="l01395"></a>01395 <span class="comment">    for(i=0; i&lt;m; ++i)</span>
<a name="l01396"></a>01396 <span class="comment">      x[i]+=alpha*d[i];</span>
<a name="l01397"></a>01397 <span class="comment">***/</span>
<a name="l01398"></a>01398     daxpy(m, x, x, alpha, d);
<a name="l01399"></a>01399 
<a name="l01400"></a>01400     <span class="keywordflow">if</span>(!(iter%50)){
<a name="l01401"></a>01401             <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){ <span class="comment">// accurate computation of the residual vector</span>
<a name="l01402"></a>01402         aim=a+i*m;
<a name="l01403"></a>01403 <span class="comment">/***</span>
<a name="l01404"></a>01404 <span class="comment">        for(j=0, sum=0.0; j&lt;m; ++j)</span>
<a name="l01405"></a>01405 <span class="comment">          sum+=aim[j]*x[j];</span>
<a name="l01406"></a>01406 <span class="comment">***/</span>
<a name="l01407"></a>01407         res[i]=B[i]-dprod(m, aim, x); <span class="comment">//B[i]-sum;</span>
<a name="l01408"></a>01408       }
<a name="l01409"></a>01409       rec_res=0;
<a name="l01410"></a>01410     }
<a name="l01411"></a>01411     <span class="keywordflow">else</span>{
<a name="l01412"></a>01412 <span class="comment">/***</span>
<a name="l01413"></a>01413 <span class="comment">            for(i=0; i&lt;m; ++i) // approximate computation of the residual vector</span>
<a name="l01414"></a>01414 <span class="comment">        res[i]-=alpha*q[i];</span>
<a name="l01415"></a>01415 <span class="comment">***/</span>
<a name="l01416"></a>01416       daxpy(m, res, res, -alpha, q);
<a name="l01417"></a>01417       rec_res=1;
<a name="l01418"></a>01418     }
<a name="l01419"></a>01419 
<a name="l01420"></a>01420     <span class="keywordflow">if</span>(prec){
<a name="l01421"></a>01421       <span class="keywordflow">switch</span>(prec){
<a name="l01422"></a>01422       <span class="keywordflow">case</span> SBA_CG_JACOBI: <span class="comment">// jacobi</span>
<a name="l01423"></a>01423         <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l01424"></a>01424           s[i]=res[i]*wk[i];
<a name="l01425"></a>01425         <span class="keywordflow">break</span>;
<a name="l01426"></a>01426       <span class="keywordflow">case</span> SBA_CG_SSOR: <span class="comment">// SSOR</span>
<a name="l01427"></a>01427         <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){
<a name="l01428"></a>01428           <span class="keywordflow">for</span>(j=0, sum=0.0, aim=a+i*m; j&lt;i; ++j)
<a name="l01429"></a>01429             sum+=aim[j]*z[j];
<a name="l01430"></a>01430           z[i]=wk[i]*(res[i]-sum);
<a name="l01431"></a>01431         }
<a name="l01432"></a>01432 
<a name="l01433"></a>01433         <span class="keywordflow">for</span>(i=m-1; i&gt;=0; --i){
<a name="l01434"></a>01434           <span class="keywordflow">for</span>(j=i+1, sum=0.0, aim=a+i*m; j&lt;m; ++j)
<a name="l01435"></a>01435             sum+=aim[j]*s[j];
<a name="l01436"></a>01436           s[i]=z[i]-wk[i]*sum;
<a name="l01437"></a>01437         }
<a name="l01438"></a>01438         <span class="keywordflow">break</span>;
<a name="l01439"></a>01439       }
<a name="l01440"></a>01440     }
<a name="l01441"></a>01441 
<a name="l01442"></a>01442     deltaold=deltanew;
<a name="l01443"></a>01443 <span class="comment">/***</span>
<a name="l01444"></a>01444 <span class="comment">          for(i=0, sum=0.0; i&lt;m; ++i)</span>
<a name="l01445"></a>01445 <span class="comment">      sum+=res[i]*s[i];</span>
<a name="l01446"></a>01446 <span class="comment">***/</span>
<a name="l01447"></a>01447     deltanew=dprod(m, res, s); <span class="comment">//sum;</span>
<a name="l01448"></a>01448 
<a name="l01449"></a>01449     <span class="comment">/* make sure that we get around small delta that are due to</span>
<a name="l01450"></a>01450 <span class="comment">     * accumulated floating point roundoff errors</span>
<a name="l01451"></a>01451 <span class="comment">     */</span>
<a name="l01452"></a>01452     <span class="keywordflow">if</span>(rec_res &amp;&amp; deltanew&lt;=eps_sq*delta0){
<a name="l01453"></a>01453       <span class="comment">/* analytically recompute delta */</span>
<a name="l01454"></a>01454             <span class="keywordflow">for</span>(i=0; i&lt;m; ++i){
<a name="l01455"></a>01455         <span class="keywordflow">for</span>(j=0, aim=a+i*m, sum=0.0; j&lt;m; ++j)
<a name="l01456"></a>01456           sum+=aim[j]*x[j];
<a name="l01457"></a>01457         res[i]=B[i]-sum;
<a name="l01458"></a>01458       }
<a name="l01459"></a>01459       deltanew=dprod(m, res, s);
<a name="l01460"></a>01460     }
<a name="l01461"></a>01461 
<a name="l01462"></a>01462     beta=deltanew/deltaold;
<a name="l01463"></a>01463 
<a name="l01464"></a>01464 <span class="comment">/***</span>
<a name="l01465"></a>01465 <span class="comment">          for(i=0; i&lt;m; ++i)</span>
<a name="l01466"></a>01466 <span class="comment">      d[i]=s[i]+beta*d[i];</span>
<a name="l01467"></a>01467 <span class="comment">***/</span>
<a name="l01468"></a>01468     daxpy(m, d, s, beta, d);
<a name="l01469"></a>01469   }
<a name="l01470"></a>01470 
<a name="l01471"></a>01471         <span class="keywordflow">return</span> iter;
<a name="l01472"></a>01472 }
<a name="l01473"></a>01473 
<a name="l01474"></a>01474 <span class="comment">/*</span>
<a name="l01475"></a>01475 <span class="comment"> * This function computes the Cholesky decomposition of the inverse of a symmetric</span>
<a name="l01476"></a>01476 <span class="comment"> * (covariance) matrix A into B, i.e. B is s.t. A^-1=B^t*B and B upper triangular.</span>
<a name="l01477"></a>01477 <span class="comment"> * A and B can coincide</span>
<a name="l01478"></a>01478 <span class="comment"> *</span>
<a name="l01479"></a>01479 <span class="comment"> * The function returns 0 in case of error (e.g. A is singular),</span>
<a name="l01480"></a>01480 <span class="comment"> * 1 if successfull</span>
<a name="l01481"></a>01481 <span class="comment"> *</span>
<a name="l01482"></a>01482 <span class="comment"> * This function is often called repetitively to operate on matrices of identical</span>
<a name="l01483"></a>01483 <span class="comment"> * dimensions. To avoid repetitive malloc&#39;s and free&#39;s, allocated memory is</span>
<a name="l01484"></a>01484 <span class="comment"> * retained between calls and free&#39;d-malloc&#39;ed when not of the appropriate size.</span>
<a name="l01485"></a>01485 <span class="comment"> * A call with NULL as the first argument forces this memory to be released.</span>
<a name="l01486"></a>01486 <span class="comment"> *</span>
<a name="l01487"></a>01487 <span class="comment"> */</span>
<a name="l01488"></a>01488 <span class="preprocessor">#if 0</span>
<a name="l01489"></a>01489 <span class="preprocessor"></span><span class="keywordtype">int</span> sba_mat_cholinv(<span class="keywordtype">double</span> *A, <span class="keywordtype">double</span> *B, <span class="keywordtype">int</span> m)
<a name="l01490"></a>01490 {
<a name="l01491"></a>01491 <span class="keyword">static</span> <span class="keywordtype">double</span> *buf=NULL;
<a name="l01492"></a>01492 <span class="keyword">static</span> <span class="keywordtype">int</span> buf_sz=0, nb=0;
<a name="l01493"></a>01493 
<a name="l01494"></a>01494 <span class="keywordtype">int</span> a_sz, ipiv_sz, work_sz, tot_sz;
<a name="l01495"></a>01495 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l01496"></a>01496 <span class="keywordtype">int</span> info, *ipiv;
<a name="l01497"></a>01497 <span class="keywordtype">double</span> *a, *work;
<a name="l01498"></a>01498    
<a name="l01499"></a>01499   <span class="keywordflow">if</span>(A==NULL){
<a name="l01500"></a>01500     <span class="keywordflow">if</span>(buf) free(buf);
<a name="l01501"></a>01501     buf=NULL;
<a name="l01502"></a>01502     buf_sz=0;
<a name="l01503"></a>01503 
<a name="l01504"></a>01504     <span class="keywordflow">return</span> 1;
<a name="l01505"></a>01505   }
<a name="l01506"></a>01506 
<a name="l01507"></a>01507   <span class="comment">/* calculate the required memory size */</span>
<a name="l01508"></a>01508   ipiv_sz=m;
<a name="l01509"></a>01509   a_sz=m*m;
<a name="l01510"></a>01510   <span class="keywordflow">if</span>(!nb){
<a name="l01511"></a>01511 <span class="preprocessor">#ifndef SBA_LS_SCARCE_MEMORY</span>
<a name="l01512"></a>01512 <span class="preprocessor"></span>    <span class="keywordtype">double</span> tmp;
<a name="l01513"></a>01513 
<a name="l01514"></a>01514     work_sz=-1; <span class="comment">// workspace query; optimal size is returned in tmp</span>
<a name="l01515"></a>01515     F77_FUNC(dgetri)((<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">int</span> *)&amp;m, NULL, (<span class="keywordtype">double</span> *)&amp;tmp, (<span class="keywordtype">int</span> *)&amp;work_sz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l01516"></a>01516     nb=((int)tmp)/m; <span class="comment">// optimal worksize is m*nb</span>
<a name="l01517"></a>01517 <span class="preprocessor">#else</span>
<a name="l01518"></a>01518 <span class="preprocessor"></span>    nb=1; <span class="comment">// min worksize is m</span>
<a name="l01519"></a>01519 <span class="preprocessor">#endif </span><span class="comment">/* SBA_LS_SCARCE_MEMORY */</span>
<a name="l01520"></a>01520   }
<a name="l01521"></a>01521   work_sz=nb*m;
<a name="l01522"></a>01522   tot_sz=(a_sz + work_sz)*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>) + ipiv_sz*<span class="keyword">sizeof</span>(int); <span class="comment">/* should be arranged in that order for proper doubles alignment */</span>
<a name="l01523"></a>01523 
<a name="l01524"></a>01524   <span class="keywordflow">if</span>(tot_sz&gt;buf_sz){ <span class="comment">/* insufficient memory, allocate a &quot;big&quot; memory chunk at once */</span>
<a name="l01525"></a>01525     <span class="keywordflow">if</span>(buf) free(buf); <span class="comment">/* free previously allocated memory */</span>
<a name="l01526"></a>01526 
<a name="l01527"></a>01527     buf_sz=tot_sz;
<a name="l01528"></a>01528     buf=(<span class="keywordtype">double</span> *)malloc(buf_sz);
<a name="l01529"></a>01529     <span class="keywordflow">if</span>(!buf){
<a name="l01530"></a>01530       fprintf(stderr, <span class="stringliteral">&quot;memory allocation in sba_mat_cholinv() failed!\n&quot;</span>);
<a name="l01531"></a>01531       exit(1);
<a name="l01532"></a>01532     }
<a name="l01533"></a>01533   }
<a name="l01534"></a>01534 
<a name="l01535"></a>01535   a=buf;
<a name="l01536"></a>01536   work=a+a_sz;
<a name="l01537"></a>01537   ipiv=(<span class="keywordtype">int</span> *)(work+work_sz);
<a name="l01538"></a>01538 
<a name="l01539"></a>01539   <span class="comment">/* step 1: invert A */</span>
<a name="l01540"></a>01540   <span class="comment">/* store A into a; A is assumed symmetric, hence no transposition is needed */</span>
<a name="l01541"></a>01541   <span class="keywordflow">for</span>(i=0; i&lt;m*m; ++i)
<a name="l01542"></a>01542     a[i]=A[i];
<a name="l01543"></a>01543 
<a name="l01544"></a>01544   <span class="comment">/* LU decomposition for A (Cholesky should also do) */</span>
<a name="l01545"></a>01545         F77_FUNC(dgetf2)((<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, ipiv, (<span class="keywordtype">int</span> *)&amp;info);  
<a name="l01546"></a>01546         <span class="comment">//F77_FUNC(dgetrf)((int *)&amp;m, (int *)&amp;m, a, (int *)&amp;m, ipiv, (int *)&amp;info);  </span>
<a name="l01547"></a>01547         <span class="keywordflow">if</span>(info!=0){
<a name="l01548"></a>01548                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l01549"></a>01549                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dgetf2/dgetrf illegal in sba_mat_cholinv()\n&quot;</span>, -info);
<a name="l01550"></a>01550                         exit(1);
<a name="l01551"></a>01551                 }
<a name="l01552"></a>01552                 <span class="keywordflow">else</span>{
<a name="l01553"></a>01553                         fprintf(stderr, <span class="stringliteral">&quot;singular matrix A for dgetf2/dgetrf in sba_mat_cholinv()\n&quot;</span>);
<a name="l01554"></a>01554                         <span class="keywordflow">return</span> 0;
<a name="l01555"></a>01555                 }
<a name="l01556"></a>01556         }
<a name="l01557"></a>01557 
<a name="l01558"></a>01558   <span class="comment">/* (A)^{-1} from LU */</span>
<a name="l01559"></a>01559         F77_FUNC(dgetri)((<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, ipiv, work, (<span class="keywordtype">int</span> *)&amp;work_sz, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l01560"></a>01560         <span class="keywordflow">if</span>(info!=0){
<a name="l01561"></a>01561                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l01562"></a>01562                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dgetri illegal in sba_mat_cholinv()\n&quot;</span>, -info);
<a name="l01563"></a>01563                         exit(1);
<a name="l01564"></a>01564                 }
<a name="l01565"></a>01565                 <span class="keywordflow">else</span>{
<a name="l01566"></a>01566                         fprintf(stderr, <span class="stringliteral">&quot;singular matrix A for dgetri in sba_mat_cholinv()\n&quot;</span>);
<a name="l01567"></a>01567                         <span class="keywordflow">return</span> 0;
<a name="l01568"></a>01568                 }
<a name="l01569"></a>01569         }
<a name="l01570"></a>01570 
<a name="l01571"></a>01571   <span class="comment">/* (A)^{-1} is now in a (in column major!) */</span>
<a name="l01572"></a>01572 
<a name="l01573"></a>01573   <span class="comment">/* step 2: Cholesky decomposition of a: A^-1=B^t B, B upper triangular */</span>
<a name="l01574"></a>01574   F77_FUNC(dpotf2)(<span class="stringliteral">&quot;U&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l01575"></a>01575   <span class="comment">/* error treatment */</span>
<a name="l01576"></a>01576   <span class="keywordflow">if</span>(info!=0){
<a name="l01577"></a>01577                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l01578"></a>01578       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dpotf2 in sba_mat_cholinv()\n&quot;</span>, -info);
<a name="l01579"></a>01579                   exit(1);
<a name="l01580"></a>01580                 }
<a name="l01581"></a>01581                 <span class="keywordflow">else</span>{
<a name="l01582"></a>01582                         fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: the leading minor of order %d is not positive definite,\n%s\n&quot;</span>, info,
<a name="l01583"></a>01583                                                   <span class="stringliteral">&quot;and the Cholesky factorization could not be completed in sba_mat_cholinv()&quot;</span>);
<a name="l01584"></a>01584                         <span class="keywordflow">return</span> 0;
<a name="l01585"></a>01585                 }
<a name="l01586"></a>01586   }
<a name="l01587"></a>01587 
<a name="l01588"></a>01588   <span class="comment">/* the decomposition is in the upper part of a (in column-major order!).</span>
<a name="l01589"></a>01589 <span class="comment">   * copying it to the lower part and zeroing the upper transposes</span>
<a name="l01590"></a>01590 <span class="comment">   * a in row-major order</span>
<a name="l01591"></a>01591 <span class="comment">   */</span>
<a name="l01592"></a>01592   <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l01593"></a>01593     <span class="keywordflow">for</span>(j=0; j&lt;i; ++j){
<a name="l01594"></a>01594       a[i+j*m]=a[j+i*m];
<a name="l01595"></a>01595       a[j+i*m]=0.0;
<a name="l01596"></a>01596     }
<a name="l01597"></a>01597   <span class="keywordflow">for</span>(i=0; i&lt;m*m; ++i)
<a name="l01598"></a>01598     B[i]=a[i];
<a name="l01599"></a>01599 
<a name="l01600"></a>01600         <span class="keywordflow">return</span> 1;
<a name="l01601"></a>01601 }
<a name="l01602"></a>01602 <span class="preprocessor">#endif</span>
<a name="l01603"></a>01603 <span class="preprocessor"></span>
<a name="l01604"></a>01604 <span class="keywordtype">int</span> sba_mat_cholinv(<span class="keywordtype">double</span> *A, <span class="keywordtype">double</span> *B, <span class="keywordtype">int</span> m)
<a name="l01605"></a>01605 {
<a name="l01606"></a>01606 <span class="keywordtype">int</span> a_sz;
<a name="l01607"></a>01607 <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l01608"></a>01608 <span class="keywordtype">int</span> info;
<a name="l01609"></a>01609 <span class="keywordtype">double</span> *a;
<a name="l01610"></a>01610    
<a name="l01611"></a>01611   <span class="keywordflow">if</span>(A==NULL){
<a name="l01612"></a>01612     <span class="keywordflow">return</span> 1;
<a name="l01613"></a>01613   }
<a name="l01614"></a>01614 
<a name="l01615"></a>01615   a_sz=m*m;
<a name="l01616"></a>01616   a=B; <span class="comment">/* use B as working memory, result is produced in it */</span>
<a name="l01617"></a>01617 
<a name="l01618"></a>01618   <span class="comment">/* step 1: invert A */</span>
<a name="l01619"></a>01619   <span class="comment">/* store A into a; A is assumed symmetric, hence no transposition is needed */</span>
<a name="l01620"></a>01620   <span class="keywordflow">for</span>(i=0; i&lt;a_sz; ++i)
<a name="l01621"></a>01621     a[i]=A[i];
<a name="l01622"></a>01622 
<a name="l01623"></a>01623   <span class="comment">/* Cholesky decomposition for A */</span>
<a name="l01624"></a>01624   F77_FUNC(dpotf2)(<span class="stringliteral">&quot;U&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l01625"></a>01625         <span class="keywordflow">if</span>(info!=0){
<a name="l01626"></a>01626                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l01627"></a>01627                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dpotf2 illegal in sba_mat_cholinv()\n&quot;</span>, -info);
<a name="l01628"></a>01628                         exit(1);
<a name="l01629"></a>01629                 }
<a name="l01630"></a>01630                 <span class="keywordflow">else</span>{
<a name="l01631"></a>01631                         fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: the leading minor of order %d is not positive definite,\n%s\n&quot;</span>, info,
<a name="l01632"></a>01632                                                   <span class="stringliteral">&quot;and the Cholesky factorization could not be completed in sba_mat_cholinv()&quot;</span>);
<a name="l01633"></a>01633                         <span class="keywordflow">return</span> 0;
<a name="l01634"></a>01634                 }
<a name="l01635"></a>01635         }
<a name="l01636"></a>01636 
<a name="l01637"></a>01637   <span class="comment">/* (A)^{-1} from Cholesky */</span>
<a name="l01638"></a>01638         F77_FUNC(dpotri)(<span class="stringliteral">&quot;U&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l01639"></a>01639         <span class="keywordflow">if</span>(info!=0){
<a name="l01640"></a>01640                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l01641"></a>01641                         fprintf(stderr, <span class="stringliteral">&quot;argument %d of dpotri illegal in sba_mat_cholinv()\n&quot;</span>, -info);
<a name="l01642"></a>01642                         exit(1);
<a name="l01643"></a>01643                 }
<a name="l01644"></a>01644                 <span class="keywordflow">else</span>{
<a name="l01645"></a>01645       fprintf(stderr, <span class="stringliteral">&quot;the (%d, %d) element of the factor U or L is zero, singular matrix A for dpotri in sba_mat_cholinv()\n&quot;</span>, info, info);
<a name="l01646"></a>01646                         <span class="keywordflow">return</span> 0;
<a name="l01647"></a>01647                 }
<a name="l01648"></a>01648         }
<a name="l01649"></a>01649 
<a name="l01650"></a>01650   <span class="comment">/* (A)^{-1} is now in a (in column major!) */</span>
<a name="l01651"></a>01651 
<a name="l01652"></a>01652   <span class="comment">/* step 2: Cholesky decomposition of a: A^-1=B^t B, B upper triangular */</span>
<a name="l01653"></a>01653   F77_FUNC(dpotf2)(<span class="stringliteral">&quot;U&quot;</span>, (<span class="keywordtype">int</span> *)&amp;m, a, (<span class="keywordtype">int</span> *)&amp;m, (<span class="keywordtype">int</span> *)&amp;info);
<a name="l01654"></a>01654   <span class="comment">/* error treatment */</span>
<a name="l01655"></a>01655   <span class="keywordflow">if</span>(info!=0){
<a name="l01656"></a>01656                 <span class="keywordflow">if</span>(info&lt;0){
<a name="l01657"></a>01657       fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: illegal value for argument %d of dpotf2 in sba_mat_cholinv()\n&quot;</span>, -info);
<a name="l01658"></a>01658                   exit(1);
<a name="l01659"></a>01659                 }
<a name="l01660"></a>01660                 <span class="keywordflow">else</span>{
<a name="l01661"></a>01661                         fprintf(stderr, <span class="stringliteral">&quot;LAPACK error: the leading minor of order %d is not positive definite,\n%s\n&quot;</span>, info,
<a name="l01662"></a>01662                                                   <span class="stringliteral">&quot;and the Cholesky factorization could not be completed in sba_mat_cholinv()&quot;</span>);
<a name="l01663"></a>01663                         <span class="keywordflow">return</span> 0;
<a name="l01664"></a>01664                 }
<a name="l01665"></a>01665   }
<a name="l01666"></a>01666 
<a name="l01667"></a>01667   <span class="comment">/* the decomposition is in the upper part of a (in column-major order!).</span>
<a name="l01668"></a>01668 <span class="comment">   * copying it to the lower part and zeroing the upper transposes</span>
<a name="l01669"></a>01669 <span class="comment">   * a in row-major order</span>
<a name="l01670"></a>01670 <span class="comment">   */</span>
<a name="l01671"></a>01671   <span class="keywordflow">for</span>(i=0; i&lt;m; ++i)
<a name="l01672"></a>01672     <span class="keywordflow">for</span>(j=0; j&lt;i; ++j){
<a name="l01673"></a>01673       a[i+j*m]=a[j+i*m];
<a name="l01674"></a>01674       a[j+i*m]=0.0;
<a name="l01675"></a>01675     }
<a name="l01676"></a>01676 
<a name="l01677"></a>01677         <span class="keywordflow">return</span> 1;
<a name="l01678"></a>01678 }
</pre></div></div>
</td></tr></table>

<br /><hr><br />
<center><a href="http://perception.inf.um.es/QVision">QVision framework</a>.
<a href="http://perception.inf.um.es">PARP research group</a>.
Copyright &copy; 2007, 2008, 2009, 2010, 2011.</center>
<br />
</body>
</html>
